Merge pull request #231 from FS-Driverless/ned-to-enu

Change codebase to use ENU instead of NED
FS-Driverless · Aug 27, 2020 · 3c1e63f · 3c1e63f
2 parents 9274890 + b456096
commit 3c1e63f
Show file tree

Hide file tree

Showing 39 changed files with 494 additions and 577 deletions.
diff --git a/AirSim/AirLib.vcxproj b/AirSim/AirLib.vcxproj
@@ -82,7 +82,7 @@
  <ClCompile Include="unreal\plugins\airsim\source\multirotor\FlyingPawn.cpp" />
  <ClCompile Include="Unreal\Plugins\AirSim\Source\Multirotor\MultirotorSimApi.cpp" />
  <ClCompile Include="Unreal\Plugins\AirSim\Source\Multirotor\SimModeWorldMultirotor.cpp" />
- <ClCompile Include="unreal\plugins\airsim\source\NedTransform.cpp" />
+ <ClCompile Include="unreal\plugins\airsim\source\CoordFrameTransformer.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\PIPCamera.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\RenderRequest.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\simhud\SimHUD.cpp" />
@@ -114,7 +114,7 @@
  <ClInclude Include="unreal\plugins\airsim\source\multirotor\MultiRotorConnector.h" />
  <ClInclude Include="Unreal\Plugins\AirSim\Source\Multirotor\MultirotorSimApi.h" />
  <ClInclude Include="unreal\plugins\airsim\source\multirotor\SimModeWorldMultiRotor.h" />
- <ClInclude Include="unreal\plugins\airsim\source\NedTransform.h" />
+ <ClInclude Include="unreal\plugins\airsim\source\CoordFrameTransformer.h" />
  <ClInclude Include="unreal\plugins\airsim\source\PIPCamera.h" />
  <ClInclude Include="unreal\plugins\airsim\source\RenderRequest.h" />
  <ClInclude Include="unreal\plugins\airsim\source\simhud\SimHUD.h" />

diff --git a/AirSim/AirLib/AirLib.vcxproj b/AirSim/AirLib/AirLib.vcxproj
@@ -70,7 +70,6 @@
  <ClInclude Include="include\common\FirstOrderFilter.hpp" />
  <ClInclude Include="include\common\FrequencyLimiter.hpp" />
  <ClInclude Include="include\common\GaussianMarkov.hpp" />
- <ClInclude Include="include\common\GeodeticConverter.hpp" />
  <ClInclude Include="include\common\LogFileWriter.hpp" />
  <ClInclude Include="include\common\ScalableClock.hpp" />
  <ClInclude Include="include\common\UpdatableContainer.hpp" />

diff --git a/AirSim/AirLib/include/common/AirSimSettings.hpp b/AirSim/AirLib/include/common/AirSimSettings.hpp
@@ -208,7 +208,7 @@ struct AirSimSettings {
  bool is_fpv_vehicle = false;
 
  //nan means use player start
- Vector3r position = VectorMath::nanVector(); //in global NED
+ Vector3r position = VectorMath::nanVector(); //in global
  Rotation rotation = Rotation::nanRotation();
 
  std::map<std::string, CameraSetting> cameras;

diff --git a/AirSim/AirLib/include/common/EarthUtils.hpp b/AirSim/AirLib/include/common/EarthUtils.hpp
@@ -274,14 +274,12 @@ class EarthUtils {
  double sin_c = sin(c), cos_c = cos(c);
  double lat_rad, lon_rad;
  if (!Utils::isApproximatelyZero(c)) { //avoids large changes?
- lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c);
- lon_rad = (home_geo_point.lon_rad + 
- atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - x_rad * home_geo_point.sin_lat * sin_c));
+ lat_rad = asin(cos_c * home_geo_point.sin_lat + (y_rad * sin_c * home_geo_point.cos_lat) / c);
+ lon_rad = (home_geo_point.lon_rad + atan2(x_rad * sin_c, c * home_geo_point.cos_lat * cos_c - y_rad * home_geo_point.sin_lat * sin_c));
 
- return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), 
- home_geo_point.home_geo_point.altitude - v.z());
+ return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), home_geo_point.home_geo_point.altitude + v.z());
  } else
- return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z());
+ return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude + v.z());
  }
 
  //below are approximate versions and would produce errors of more than 10m for points farther than 1km

diff --git a/AirSim/AirLib/include/common/GeodeticConverter.hpp b/AirSim/AirLib/include/common/GeodeticConverter.hpp
diff --git a/AirSim/AirLib/include/common/VectorMath.hpp b/AirSim/AirLib/include/common/VectorMath.hpp
@@ -326,9 +326,11 @@ class VectorMathT {
  {
  // these values are rotations between -pi and pi
 
+ // pitch_start, roll_start, yaw_start
  RealT p_s, r_s, y_s;
  toEulerianAngle(start, p_s, r_s, y_s);
 
+ // pitch_end, roll_end, yaw_end
  // these values are rotations between -pi and pi.
  RealT p_e, r_e, y_e;
  toEulerianAngle(end, p_e, r_e, y_e);

diff --git a/AirSim/AirLib/include/sensors/SensorFactory.hpp b/AirSim/AirLib/include/sensors/SensorFactory.hpp
@@ -9,6 +9,7 @@
 //sensors
 #include "sensors/imu/ImuSimple.hpp"
 #include "sensors/gps/GpsSimple.hpp"
+#include "sensors/gss/GSSSimple.hpp"
 
 namespace msr { namespace airlib {
 

diff --git a/AirSim/AirLib/include/sensors/lidar/LidarBase.hpp b/AirSim/AirLib/include/sensors/lidar/LidarBase.hpp
@@ -25,7 +25,7 @@ class LidarBase : public SensorBase {
  // - array of floats that represent [x,y,z] coordinate for each point hit within the range
  // x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
  // TODO: Do we need an intensity place-holder [x,y,z, intensity]?
- // - in lidar local NED coordinates
+ // - in lidar local coordinates
  // - in meters
  vector<real_T> point_cloud;
  };

diff --git a/AirSim/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirSim/AirLib/include/sensors/lidar/LidarSimple.hpp
@@ -68,14 +68,7 @@ class LidarSimple : public LidarBase {
 
  const GroundTruth& ground_truth = getGroundTruth();
 
- // calculate the pose before obtaining the point-cloud. Before/after is a bit arbitrary
- // decision here. If the pose can change while obtaining the point-cloud (could happen for drones)
- // then the pose won't be very accurate either way.
- //
- // TODO: Seems like pose is in vehicle inertial-frame (NOT in Global NED frame).
- // That could be a bit unintuitive but seems consistent with the position/orientation returned as part of 
- // ImageResponse for cameras and pose returned by getCameraInfo API.
- // Do we need to convert pose to Global NED frame before returning to clients?
+ // we can just add these two poses because they are in the same coordinate frame :)
  Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose;
  getPointCloud(params_.relative_pose, // relative lidar pose
  ground_truth.kinematics->pose, // relative vehicle pose

diff --git a/AirSim/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirSim/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
@@ -22,9 +22,9 @@ struct LidarSimpleParams {
  real_T vertical_FOV_lower = -45;
 
  Pose relative_pose {
- Vector3r(0,0,-1), // position - a little above vehicle
+ Vector3r(0, 0, 1), // position - a little above vehicle
  Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0) 
-  };  
+ }; 
 
  bool draw_debug_points = false;
 
@@ -56,7 +56,7 @@ struct LidarSimpleParams {
  if (std::isnan(relative_pose.position.y()))
  relative_pose.position.y() = 0;
  if (std::isnan(relative_pose.position.z())) {
- relative_pose.position.z() = -1; // a little bit above for cars
+ relative_pose.position.z() = 1; // a little bit above for cars
  }
 
  float pitch, roll, yaw;

diff --git a/AirSim/PythonClient/airsim/client.py b/AirSim/PythonClient/airsim/client.py
@@ -505,7 +505,7 @@ def simFlushPersistentMarkers(self):
 
  def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False):
  """
- Plot a list of 3D points in World NED frame
+ Plot a list of 3D points in World frame
 
  Args:
  points (list[Vector3r]): List of Vector3r objects
@@ -518,7 +518,7 @@ def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, du
 
  def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False):
  """
- Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1]
+ Plots a line strip in World frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1]
 
  Args:
  points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects

diff --git a/AirSim/UnrealPluginFiles.vcxproj b/AirSim/UnrealPluginFiles.vcxproj
@@ -82,7 +82,7 @@
  <ClCompile Include="unreal\plugins\airsim\source\multirotor\FlyingPawn.cpp" />
  <ClCompile Include="Unreal\Plugins\AirSim\Source\Multirotor\MultirotorSimApi.cpp" />
  <ClCompile Include="Unreal\Plugins\AirSim\Source\Multirotor\SimModeWorldMultirotor.cpp" />
- <ClCompile Include="unreal\plugins\airsim\source\NedTransform.cpp" />
+ <ClCompile Include="unreal\plugins\airsim\source\CoordFrameTransformer.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\PIPCamera.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\RenderRequest.cpp" />
  <ClCompile Include="unreal\plugins\airsim\source\simhud\SimHUD.cpp" />
@@ -114,7 +114,7 @@
  <ClInclude Include="unreal\plugins\airsim\source\multirotor\MultiRotorConnector.h" />
  <ClInclude Include="Unreal\Plugins\AirSim\Source\Multirotor\MultirotorSimApi.h" />
  <ClInclude Include="unreal\plugins\airsim\source\multirotor\SimModeWorldMultiRotor.h" />
- <ClInclude Include="unreal\plugins\airsim\source\NedTransform.h" />
+ <ClInclude Include="unreal\plugins\airsim\source\CoordFrameTransformer.h" />
  <ClInclude Include="unreal\plugins\airsim\source\PIPCamera.h" />
  <ClInclude Include="unreal\plugins\airsim\source\RenderRequest.h" />
  <ClInclude Include="unreal\plugins\airsim\source\simhud\SimHUD.h" />

diff --git a/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj b/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj
@@ -70,7 +70,6 @@
  <ClInclude Include="include\common\FirstOrderFilter.hpp" />
  <ClInclude Include="include\common\FrequencyLimiter.hpp" />
  <ClInclude Include="include\common\GaussianMarkov.hpp" />
- <ClInclude Include="include\common\GeodeticConverter.hpp" />
  <ClInclude Include="include\common\LogFileWriter.hpp" />
  <ClInclude Include="include\common\ScalableClock.hpp" />
  <ClInclude Include="include\common\UpdatableContainer.hpp" />