Add support for bridging NavSat

ros_ign_bridge/README.md

@@ -37,6 +37,7 @@ service calls. Its support is limited to only the following message types:
 | sensor_msgs/JointState         | ignition::msgs::Model            |
 | sensor_msgs/LaserScan          | ignition::msgs::LaserScan        |
 | sensor_msgs/MagneticField      | ignition::msgs::Magnetometer     |
+| sensor_msgs/NavSatFix          | ignition::msgs::NavSat           |
 | sensor_msgs/PointCloud2        | ignition::msgs::PointCloudPacked |
 | tf_msgs/TFMessage              | ignition::msgs::Pose_V           |
 | visualization_msgs/Marker      | ignition::msgs::Marker           |

ros_ign_bridge/include/ros_ign_bridge/convert.hpp

@@ -37,6 +37,7 @@
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/ColorRGBA.h>
@@ -412,6 +413,18 @@ convert_ign_to_ros(
   const ignition::msgs::Magnetometer & ign_msg,
   sensor_msgs::MagneticField & ros_msg);
 
+template<>
+void
+convert_ros_to_ign(
+  const sensor_msgs::NavSatFix & ros_msg,
+  ignition::msgs::NavSat & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::NavSat & ign_msg,
+  sensor_msgs::NavSatFix & ros_msg);
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/src/convert.cpp

@@ -1141,6 +1141,41 @@ convert_ign_to_ros(
   // magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer.
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const sensor_msgs::NavSatFix & ros_msg,
+  ignition::msgs::NavSat & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
+  ign_msg.set_latitude_deg(ros_msg.latitude);
+  ign_msg.set_longitude_deg(ros_msg.longitude);
+  ign_msg.set_altitude(ros_msg.altitude);
+  ign_msg.set_frame_id(ros_msg.header.frame_id);
+
+  // Not supported in sensor_msgs::NavSatFix.
+  ign_msg.set_velocity_east(0.0);
+  ign_msg.set_velocity_north(0.0);
+  ign_msg.set_velocity_up(0.0);
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::NavSat & ign_msg,
+  sensor_msgs::NavSatFix & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
+  ros_msg.header.frame_id = frame_id_ign_to_ros(ign_msg.frame_id());
+  ros_msg.latitude = ign_msg.latitude_deg();
+  ros_msg.longitude = ign_msg.longitude_deg();
+  ros_msg.altitude = ign_msg.altitude();
+
+  // position_covariance is not supported in Ignition::Msgs::NavSat.
+  ros_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+  ros_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
+}
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/src/factories.cpp

@@ -346,6 +346,17 @@ get_factory_impl(
       >
     >("sensor_msgs/Magnetometer", ign_type_name);
   }
+  if (
+    (ros_type_name == "sensor_msgs/NavSatFix" || ros_type_name == "") &&
+     ign_type_name == "ignition.msgs.NavSat")
+  {
+    return std::make_shared<
+      Factory<
+        sensor_msgs::NavSatFix,
+        ignition::msgs::NavSat
+      >
+    >("sensor_msgs/NavSatFix", ign_type_name);
+  }
   if (
     (ros_type_name == "sensor_msgs/PointCloud2" || ros_type_name == "") &&
      ign_type_name == "ignition.msgs.PointCloudPacked")
@@ -1109,6 +1120,30 @@ Factory<
   ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
 }
 
+template<>
+void
+Factory<
+  sensor_msgs::NavSatFix,
+  ignition::msgs::NavSat
+>::convert_ros_to_ign(
+  const sensor_msgs::NavSatFix & ros_msg,
+  ignition::msgs::NavSat & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  sensor_msgs::NavSatFix,
+  ignition::msgs::NavSat
+>::convert_ign_to_ros(
+  const ignition::msgs::NavSat & ign_msg,
+  sensor_msgs::NavSatFix & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 template<>
 void
 Factory<

ros_ign_bridge/src/factories.hpp

@@ -37,6 +37,7 @@
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/ColorRGBA.h>
@@ -594,6 +595,25 @@ Factory<
   const ignition::msgs::Magnetometer & ign_msg,
   sensor_msgs::MagneticField & ros_msg);
 
+template<>
+void
+Factory<
+  sensor_msgs::NavSatFix,
+  ignition::msgs::NavSat
+>::convert_ros_to_ign(
+  const sensor_msgs::NavSatFix & ros_msg,
+  ignition::msgs::NavSat & ign_msg);
+
+template<>
+void
+Factory<
+  sensor_msgs::NavSatFix,
+  ignition::msgs::NavSat
+>::convert_ign_to_ros(
+  const ignition::msgs::NavSat & ign_msg,
+  sensor_msgs::NavSatFix & ros_msg);
+
+
 template<>
 void
 Factory<

ros_ign_bridge/test/launch/test_ign_subscriber.launch

@@ -28,6 +28,7 @@
               /imu@sensor_msgs/[email protected]
               /laserscan@sensor_msgs/[email protected]
               /magnetic@sensor_msgs/[email protected]
+              /navsat@sensor_msgs/[email protected]
               /actuators@mav_msgs/[email protected]
               /map@nav_msgs/[email protected]
               /odometry@nav_msgs/[email protected]

ros_ign_bridge/test/launch/test_ros_subscriber.launch

@@ -28,6 +28,7 @@
               /imu@sensor_msgs/[email protected]
               /laserscan@sensor_msgs/[email protected]
               /magnetic@sensor_msgs/[email protected]
+              /navsat@sensor_msgs/[email protected]
               /actuators@mav_msgs/[email protected]
               /map@nav_msgs/[email protected]
               /odometry@nav_msgs/[email protected]

ros_ign_bridge/test/publishers/ign_publisher.cpp

@@ -171,6 +171,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::Magnetometer magnetometer_msg;
   ros_ign_bridge::testing::createTestMsg(magnetometer_msg);
 
+  // ignition::msgs::NavSat.
+  auto navsat_pub = node.Advertise<ignition::msgs::NavSat>("navsat");
+  ignition::msgs::NavSat navsat_msg;
+  ros_ign_bridge::testing::createTestMsg(navsat_msg);
+
   // ignition::msgs::Actuators.
   auto actuators_pub = node.Advertise<ignition::msgs::Actuators>("actuators");
   ignition::msgs::Actuators actuators_msg;
@@ -242,6 +247,7 @@ int main(int /*argc*/, char **/*argv*/)
     imu_pub.Publish(imu_msg);
     laserscan_pub.Publish(laserscan_msg);
     magnetic_pub.Publish(magnetometer_msg);
+    navsat_pub.Publish(navsat_msg);
     actuators_pub.Publish(actuators_msg);
     map_pub.Publish(map_msg);
     odometry_pub.Publish(odometry_msg);

ros_ign_bridge/test/publishers/ros_publisher.cpp

@@ -42,6 +42,7 @@
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf2_msgs/TFMessage.h>
 #include <visualization_msgs/Marker.h>
@@ -220,6 +221,12 @@ int main(int argc, char ** argv)
   sensor_msgs::MagneticField magnetic_msg;
   ros_ign_bridge::testing::createTestMsg(magnetic_msg);
 
+  // sensor_msgs::NavSatFix.
+  ros::Publisher navsat_pub =
+    n.advertise<sensor_msgs::NavSatFix>("navsat", 1000);
+  sensor_msgs::NavSatFix navsat_msg;
+  ros_ign_bridge::testing::createTestMsg(navsat_msg);
+
   // sensor_msgs::PointCloud2.
   ros::Publisher pointcloud2_pub =
     n.advertise<sensor_msgs::PointCloud2>("pointcloud2", 1000);
@@ -275,6 +282,7 @@ int main(int argc, char ** argv)
     imu_pub.publish(imu_msg);
     laserscan_pub.publish(laserscan_msg);
     magnetic_pub.publish(magnetic_msg);
+    navsat_pub.publish(navsat_msg);
     joint_states_pub.publish(joint_states_msg);
     pointcloud2_pub.publish(pointcloud2_msg);
     battery_state_pub.publish(battery_state_msg);

ros_ign_bridge/test/subscribers/ign_subscriber.cpp

@@ -335,6 +335,18 @@ TEST(IgnSubscriberTest, Magnetometer)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, NavSat)
+{
+  MyTestClass<ignition::msgs::NavSat> client("navsat");
+
+  using namespace std::chrono_literals;
+  ros_ign_bridge::testing::waitUntilBoolVar(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(IgnSubscriberTest, Actuators)
 {

ros_ign_bridge/test/subscribers/ros_subscriber.cpp

@@ -44,6 +44,7 @@
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
@@ -391,6 +392,17 @@ TEST(ROSSubscriberTest, MagneticField)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+TEST(ROSSubscriberTest, NavSatFix)
+{
+  MyTestClass<sensor_msgs::NavSatFix> client("navsat");
+
+  using namespace std::chrono_literals;
+  ros_ign_bridge::testing::waitUntilBoolVarAndSpin(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(ROSSubscriberTest, Actuators)
 {

ros_ign_bridge/test/test_utils.h

@@ -48,6 +48,7 @@
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/PointField.h>
 #include <tf2_msgs/TFMessage.h>
@@ -832,6 +833,42 @@ namespace testing
       EXPECT_FLOAT_EQ(0, _msg.magnetic_field_covariance[i]);
   }
 
+  /// \brief Create a message used for testing.
+  /// \param[out] _msg The message populated.
+  void createTestMsg(sensor_msgs::NavSatFix &_msg)
+  {
+    std_msgs::Header header_msg;
+    createTestMsg(header_msg);
+
+    _msg.header                    = header_msg;
+    _msg.status.status             = sensor_msgs::NavSatStatus::STATUS_FIX;
+    _msg.latitude                  = 0.00;
+    _msg.longitude                 = 0.00;
+    _msg.altitude                  = 0.00;
+    _msg.position_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+    _msg.position_covariance_type  =
+      sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+  }
+
+  /// \brief Compare a message with the populated for testing.
+  /// \param[in] _msg The message to compare.
+  void compareTestMsg(const sensor_msgs::NavSatFix &_msg)
+  {
+    sensor_msgs::NavSatFix expected_msg;
+    createTestMsg(expected_msg);
+
+    compareTestMsg(_msg.header);
+    EXPECT_EQ(expected_msg.status, _msg.status);
+    EXPECT_FLOAT_EQ(expected_msg.latitude,           _msg.latitude);
+    EXPECT_FLOAT_EQ(expected_msg.longitude,          _msg.longitude);
+    EXPECT_FLOAT_EQ(expected_msg.altitude,           _msg.altitude);
+    EXPECT_EQ(expected_msg.position_covariance_type,
+      _msg.position_covariance_type);
+
+    for (auto i = 0u; i < 9; ++i)
+      EXPECT_FLOAT_EQ(0, _msg.position_covariance[i]);
+  }
+
   /// \brief Create a message used for testing.
   /// \param[out] _msg The message populated.
   void createTestMsg(sensor_msgs::PointCloud2 &_msg)
@@ -1633,6 +1670,39 @@ namespace testing
     compareTestMsg(_msg.field_tesla());
   }
 
+  /// \brief Create a message used for testing.
+  /// \param[out] _msg The message populated.
+  void createTestMsg(ignition::msgs::NavSat &_msg)
+  {
+    ignition::msgs::Header header_msg;
+    createTestMsg(header_msg);
+
+    _msg.mutable_header()->CopyFrom(header_msg);
+    _msg.set_frame_id("frame_id_value");
+    _msg.set_latitude_deg(0.00);
+    _msg.set_longitude_deg(0.00);
+    _msg.set_altitude(0.00);
+    _msg.set_velocity_east(0.00);
+    _msg.set_velocity_north(0.00);
+    _msg.set_velocity_up(0.00);
+  }
+
+  /// \brief Compare a message with the populated for testing.
+  /// \param[in] _msg The message to compare.
+  void compareTestMsg(const ignition::msgs::NavSat &_msg)
+  {
+    ignition::msgs::NavSat expected_msg;
+    createTestMsg(expected_msg);
+
+    compareTestMsg(_msg.header());
+    EXPECT_FLOAT_EQ(expected_msg.latitude_deg(),  _msg.latitude_deg());
+    EXPECT_FLOAT_EQ(expected_msg.longitude_deg(),  _msg.longitude_deg());
+    EXPECT_FLOAT_EQ(expected_msg.altitude(),  _msg.altitude());
+    EXPECT_FLOAT_EQ(expected_msg.velocity_east(),  _msg.velocity_east());
+    EXPECT_FLOAT_EQ(expected_msg.velocity_north(),  _msg.velocity_north());
+    EXPECT_FLOAT_EQ(expected_msg.velocity_up(),  _msg.velocity_up());
+  }
+
   /// \brief Create a message used for testing.
   /// \param[out] _msg The message populated.
   void createTestMsg(ignition::msgs::Actuators &_msg)

ros_ign_gazebo_demos/README.md

@@ -92,6 +92,14 @@ Publishes magnetic field readings.
 
 ![](images/magnetometer_demo.png)
 
+## GNSS
+
+Publishes satellite navigation readings.
+
+    roslaunch ros_ign_gazebo_demos navsat.launch
+
+![](images/navsat_demo.png)
+
 ## RGBD camera
 
 RGBD camera data can be obtained as:

ros_ign_gazebo_demos/launch/navsat.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+
+  <include file="$(find ros_ign_gazebo)/launch/ign_gazebo.launch">
+    <arg name="ign_args" value="-r -v 3 spherical_coordinates.sdf"/>
+  </include>
+
+  <node
+    pkg="ros_ign_bridge"
+    type="parameter_bridge"
+    name="$(anon ros_ign_bridge)"
+    output="screen"
+    args="/navsat@sensor_msgs/[email protected]">
+  </node>
+
+  <node
+    type="rqt_topic"
+    name="rqt_topic"
+    pkg="rqt_topic"
+    args="-t" />
+</launch>