Merge pull request #1 from nocs-ocs/bug_fix_gps_plugin_coordinate

Bugfix GPS plugin coordinate
nocs-ocs · Aug 8, 2024 · ef97923 · ef97923
2 parents 77a8a36 + 5bd1c1e
commit ef97923
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/uuv_sensor_plugins/uuv_sensor_ros_plugins/src/GPSROSPlugin.cc b/uuv_sensor_plugins/uuv_sensor_ros_plugins/src/GPSROSPlugin.cc
@@ -75,8 +75,8 @@ bool GPSROSPlugin::OnUpdateGPS()
   this->gpsMessage.header.stamp.nsec = currentTime.nsec;
 
   // Copy the output of Gazebo's GPS sensor into a NavSatFix message
-  this->gpsMessage.latitude = -this->gazeboGPSSensor->Latitude().Degree();
-  this->gpsMessage.longitude = -this->gazeboGPSSensor->Longitude().Degree();
+  this->gpsMessage.latitude = this->gazeboGPSSensor->Latitude().Degree();
+  this->gpsMessage.longitude = this->gazeboGPSSensor->Longitude().Degree();
   this->gpsMessage.altitude = this->gazeboGPSSensor->Altitude();
 
   this->rosSensorOutputPub.publish(this->gpsMessage);

diff --git a/uuv_world_plugins/uuv_world_ros_plugins/src/SphericalCoordinatesROSInterfacePlugin.cc b/uuv_world_plugins/uuv_world_ros_plugins/src/SphericalCoordinatesROSInterfacePlugin.cc
@@ -135,8 +135,13 @@ bool SphericalCoordinatesROSInterfacePlugin::TransformFromSphericalCoord(
   ignition::math::Vector3d cartVec =
     this->world->GetSphericalCoordinates()->LocalFromSpherical(scVec);
 #endif
-  _res.output.x = cartVec.X();
-  _res.output.y = cartVec.Y();
+  // The minus signs on X and Y come from the fact that
+  // Gazebo outputs the cartesian coordinates in the
+  // -E/-N/U format instead of E/N/U format
+  // The issue is mentioned in:
+  // https://github.com/uuvsimulator/uuv_simulator/issues/390 
+  _res.output.x = -cartVec.X();
+  _res.output.y = -cartVec.Y();
   _res.output.z = cartVec.Z();
   return true;
 }