Port NavSat (#224) from ROS 1 to ROS 2

Co-authored-by: Tyler Howell <[email protected]>
Signed-off-by: Michael Carroll <[email protected]>

ros_ign_bridge/README.md

@@ -50,6 +50,7 @@ service calls. Its support is limited to only the following message types:
 | sensor_msgs/msg/JointState           | ignition::msgs::Model                |
 | sensor_msgs/msg/LaserScan            | ignition::msgs::LaserScan            |
 | sensor_msgs/msg/MagneticField        | ignition::msgs::Magnetometer         |
+| sensor_msgs/msg/NavSatFixed          | ignition::msgs::NavSat               |
 | sensor_msgs/msg/PointCloud2          | ignition::msgs::PointCloudPacked     |
 | tf2_msgs/msg/TFMessage               | ignition::msgs::Pose_V               |
 | trajectory_msgs/msg/JointTrajectory  | ignition::msgs::JointTrajectory      |

ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp

@@ -23,6 +23,7 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 // Ignition messages
@@ -34,6 +35,7 @@
 #include <ignition/msgs/laserscan.pb.h>
 #include <ignition/msgs/magnetometer.pb.h>
 #include <ignition/msgs/model.pb.h>
+#include <ignition/msgs/navsat.pb.h>
 #include <ignition/msgs/pointcloud_packed.pb.h>
 
 #include <ros_ign_bridge/convert_decl.hpp>
@@ -126,6 +128,18 @@ convert_ign_to_ros(
   const ignition::msgs::Magnetometer & ign_msg,
   sensor_msgs::msg::MagneticField & ros_msg);
 
+template<>
+void
+convert_ros_to_ign(
+  const sensor_msgs::msg::NavSatFix & ros_msg,
+  ignition::msgs::NavSat & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::NavSat & ign_msg,
+  sensor_msgs::msg::NavSatFix & ros_msg);
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/ros_ign_bridge/mappings.py

@@ -58,15 +58,16 @@
         Mapping('Clock', 'Clock'),
     ],
     'sensor_msgs': [
+        Mapping('BatteryState', 'BatteryState'),
+        Mapping('CameraInfo', 'CameraInfo'),
         Mapping('FluidPressure', 'FluidPressure'),
         Mapping('Image', 'Image'),
-        Mapping('CameraInfo', 'CameraInfo'),
         Mapping('Imu', 'IMU'),
         Mapping('JointState', 'Model'),
         Mapping('LaserScan', 'LaserScan'),
         Mapping('MagneticField', 'Magnetometer'),
+        Mapping('NavSatFix', 'NavSat'),
         Mapping('PointCloud2', 'PointCloudPacked'),
-        Mapping('BatteryState', 'BatteryState'),
     ],
     'std_msgs': [
         Mapping('Bool', 'Boolean'),

ros_ign_bridge/src/convert/sensor_msgs.cpp

@@ -416,6 +416,41 @@ convert_ign_to_ros(
   // magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer.
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const sensor_msgs::msg::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::msg::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::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+  ros_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
+}
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/test/utils/ign_test_msg.cpp

@@ -831,6 +831,35 @@ void compareTestMsg(const std::shared_ptr<ignition::msgs::Magnetometer> & _msg)
   compareTestMsg(std::make_shared<ignition::msgs::Vector3d>(_msg->field_tesla()));
 }
 
+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);
+}
+
+void compareTestMsg(const std::shared_ptr<ignition::msgs::NavSat> & _msg)
+{
+  ignition::msgs::NavSat expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<ignition::msgs::Header>(_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());
+}
+
 void createTestMsg(ignition::msgs::Actuators & _msg)
 {
   ignition::msgs::Header header_msg;

ros_ign_bridge/test/utils/ign_test_msg.hpp

@@ -46,6 +46,7 @@
 #include <ignition/msgs/light.pb.h>
 #include <ignition/msgs/magnetometer.pb.h>
 #include <ignition/msgs/model.pb.h>
+#include <ignition/msgs/navsat.pb.h>
 #include <ignition/msgs/odometry.pb.h>
 #include <ignition/msgs/param.pb.h>
 #include <ignition/msgs/pointcloud_packed.pb.h>
@@ -325,6 +326,14 @@ void createTestMsg(ignition::msgs::Magnetometer & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ignition::msgs::Magnetometer> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(ignition::msgs::NavSat & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<ignition::msgs::NavSat> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ignition::msgs::Actuators & _msg);

ros_ign_bridge/test/utils/ros_test_msg.cpp

@@ -919,6 +919,37 @@ void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::MagneticField> & _ms
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->magnetic_field));
 }
 
+void createTestMsg(sensor_msgs::msg::NavSatFix & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+
+  _msg.header = header_msg;
+  _msg.status.status = sensor_msgs::msg::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::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+}
+
+void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::NavSatFix> & _msg)
+{
+  sensor_msgs::msg::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]);
+  }
+}
+
 void createTestMsg(sensor_msgs::msg::PointCloud2 & _msg)
 {
   createTestMsg(_msg.header);

ros_ign_bridge/test/utils/ros_test_msg.hpp

@@ -58,6 +58,7 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/msg/point_field.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
@@ -423,6 +424,14 @@ void createTestMsg(sensor_msgs::msg::MagneticField & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::MagneticField> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(sensor_msgs::msg::NavSatFix & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::NavSatFix> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(sensor_msgs::msg::PointCloud2 & _msg);

ros_ign_gazebo_demos/README.md

@@ -122,6 +122,14 @@ Publishes magnetic field readings.
 
 ![](images/magnetometer_demo.png)
 
+## GNSS
+
+Publishes satellite navigation readings, only available in Fortress on.
+
+    ros2 launch ros_ign_gazebo_demos navsat.launch.py
+
+![](images/navsat_demo.png)
+
 ## RGBD camera
 
 RGBD camera data can be obtained as:

ros_ign_gazebo_demos/launch/navsat.launch.py

@@ -0,0 +1,63 @@
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
+
+    ign_gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
+        launch_arguments={
+            'ign_args': '-v 4 -r spherical_coordinates.sdf'
+        }.items(),
+    )
+
+    # RQt
+    rqt = Node(
+        package='rqt_topic',
+        executable='rqt_topic',
+        arguments=['-t'],
+        condition=IfCondition(LaunchConfiguration('rqt'))
+    )
+
+    # Bridge
+    bridge = Node(
+        package='ros_ign_bridge',
+        executable='parameter_bridge',
+        arguments=['/navsat@sensor_msgs/msg/[email protected]'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        ign_gazebo,
+        DeclareLaunchArgument('rqt', default_value='true',
+                              description='Open RQt.'),
+        bridge,
+        rqt
+    ])