Change codebase to use ENU instead of NED

[SijmenHuizenga]

In this pr I refactored the codebase to use the ENU coordinate frames as used in the autonomous industry. See the ROS REP 103 and the Automated Driving Toolbox. This replaces the NED coordinate system introduced by using AirSim.

From now on we are using the following coordinate system This introduces the following breaking change:

• From now on, all coordinates in the settings.json should be in ENU with x forward instead of NED with x forward. This relates to all x, y, z, roll, pitch, yaw and vertical & horizontal field of views. So flip the Z axis and flip the vertical field of view upper and lower.
• From now on, yaw 0 means car facing east. (in line with ros standard). Before, yaw 0 meant car facing north.

I've tested all the things I could think of that use coordinate's and rotations. This is what I have tested:

• Odometry position
• Odometry velocity
• Odometry orientation
• GPS
• IMU Acceleration
• IMU Orientation
• IMU Angluar Velocity
• Camera positioning and rotation from settings.json to output
• Lidar positioning and rotation settings.json
• Lidar points
• ROS tf's
• GSS
• Track

Can you help with testing?

It would be great if you could checkout this code, run it and see if your autonomous system still works.
There should be no changes in the data coming out from the ros wrapper before and after this pr.

[SijmenHuizenga]

Lidar testing...

used settings.json

"LidarX": {
          "SensorType": 6,
          "Enabled": true,
          "X": 2, "Y": 0, "Z": 0.3,
          "Roll": 0, "Pitch": 0, "Yaw" : 0,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 10,
          "DrawDebugPoints": false
        },
        "LidarY": {
          "SensorType": 6,
          "Enabled": true,
          "X": 0, "Y": 2, "Z": 0.3,
          "Roll": 0, "Pitch": 0, "Yaw" : 0,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 5,
          "DrawDebugPoints": false
        },
        "LidarZ": {
          "SensorType": 6,
          "Enabled": true,
          "X": 0, "Y": 0, "Z": 2,
          "Roll": 0, "Pitch": 0, "Yaw" : 0,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 5,
          "DrawDebugPoints": false
        },
        "LidarRoll": {
          "SensorType": 6,
          "Enabled": true,
          "X": 1, "Y": 1, "Z": 0.3,
          "Roll": 60, "Pitch": 0, "Yaw" : 0,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 5,
          "DrawDebugPoints": false
        },
        "LidarPitch": {
          "SensorType": 6,
          "Enabled": true,
          "X": 1, "Y": 2, "Z": 0.3,
          "Roll": 0, "Pitch": 60, "Yaw" : 0,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 5,
          "DrawDebugPoints": false
        },
        "LidarYaw": {
          "SensorType": 6,
          "Enabled": true,
          "X": 1, "Y": 3, "Z": 0.3,
          "Roll": 0, "Pitch": 0, "Yaw" : 60,
          "NumberOfLasers": 4,
          "PointsPerScan": 4096,
          "VerticalFOVUpper": 5,
          "VerticalFOVLower": -5,
          "HorizontalFOVStart": -90,
          "HorizontalFOVEnd": 90,
          "RotationsPerSecond": 5,
          "DrawDebugPoints": false
        },

Success!

[davidoort]

This is with the same settings.json we used in competition. This is not an issue with this PR, it's just that during competition the sensors had to be placed in NED coordinates in the settings.json file. I hope this has been clearly stated somewhere.

[davidoort]

• Autonomous driving with groundtruth track and odometry works

• Autonomous driving with only groundtruth track does not work. Still don't know why but it has to be an issue with either gps, gss or IMU

[davidoort]

At the moment I can't drive autonomously with the same software stack as the one in FSOnline, so we should figure out what is going on before merging. In the picture you can see the estimated car pos vs the groundtruth car pos. During competition they were on top of each other

[davidoort]

After flipping the sign of Z for the lidar and of VerticalFOVLower (now these values are in ENU) in the settings.json file I get a pointcloud that is equal or resembles our original one

Our lidar pipeline is correctly able to do detection on this pointcloud, so all good here

[SijmenHuizenga]

@davidoort Thanks you so much for testing!

I ran some more system identification tests to validate the correctness of imu, gps and gss. I manually drove the car in a certain pattern and plotted all the things.

On master

On this branch

[davidoort]

@davidoort Thanks you so much for testing!

I ran some more system identification tests to validate the correctness of imu, gps and gss. I manually drove the car in a certain pattern and plotted all the things.

Cool! It seems like gps longitude is flipped right? that would certainly explain the issue I'm having

[SijmenHuizenga]

Yes it does!

Edit: Fixed

[davidoort]

Adjusting the initial heading of the car in our Kalman filter from North to East and flipping the z signs and pitch angle signs in our settings.json worked!