Fix for sensor pointer null when navsat plugin in included in sdf (#2176

) Signed-off-by: ashbabu <[email protected]> Signed-off-by: Michael Carroll <[email protected]> Co-authored-by: Michael Carroll <[email protected]> Co-authored-by: Ian Chen <[email protected]>
gazebosim · Oct 30, 2023 · 28728c9 · 28728c9
1 parent d7381e5
commit 28728c9
Showing 1 changed file with 35 additions and 0 deletions.
diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -1424,6 +1424,41 @@ sdf::Sensor gazebo::convert(const msgs::Sensor &_in)
 
     out.SetCameraSensor(sensor);
   }
+  else if (out.Type() == sdf::SensorType::GPS ||
+           out.Type() == sdf::SensorType::NAVSAT)
+  {
+    sdf::NavSat sensor;
+    if (_in.has_gps())
+    {
+      if (_in.gps().position().has_horizontal_noise())
+      {
+        sensor.SetHorizontalPositionNoise(gazebo::convert<sdf::Noise>(
+              _in.gps().position().horizontal_noise()));
+      }
+      if (_in.gps().position().has_vertical_noise())
+      {
+        sensor.SetVerticalPositionNoise(gazebo::convert<sdf::Noise>(
+              _in.gps().position().vertical_noise()));
+      }
+      if (_in.gps().velocity().has_horizontal_noise())
+      {
+        sensor.SetHorizontalVelocityNoise(gazebo::convert<sdf::Noise>(
+              _in.gps().velocity().horizontal_noise()));
+      }
+      if (_in.gps().velocity().has_vertical_noise())
+      {
+        sensor.SetVerticalVelocityNoise(gazebo::convert<sdf::Noise>(
+              _in.gps().velocity().vertical_noise()));
+      }
+    }
+    else
+    {
+      ignerr << "Attempting to convert a navsat sensor message, but the "
+             << "message does not have a navsat nested message.\n";
+    }
+
+    out.SetNavSatSensor(sensor);
+  }
   else if (out.Type() == sdf::SensorType::ALTIMETER)
   {
     sdf::Altimeter sensor;