[backport Humble] Create bridge for GPSFix msg (gazebosim#316) (gazebosim#538)

Signed-off-by: Vincent Rousseau <[email protected]>
Signed-off-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Rousseau Vincent <[email protected]>

Author: ahcorde

ros_gz_bridge/CMakeLists.txt

@@ -71,6 +71,7 @@ set(BRIDGE_MESSAGE_TYPES
   builtin_interfaces
   actuator_msgs
   geometry_msgs
+  gps_msgs
   nav_msgs
   rcl_interfaces
   ros_gz_interfaces

ros_gz_bridge/README.md

@@ -32,6 +32,7 @@ The following message types can be bridged for topics:
 | geometry_msgs/msg/TwistStamped              | ignition::msgs::Twist                       |
 | geometry_msgs/msg/TwistWithCovariance       | ignition::msgs::TwistWithCovariance         |
 | geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance         |
+| gps_msgs/GPSFix                             | ignition::msgs::NavSat                      |
 | nav_msgs/msg/Odometry                       | ignition::msgs::Odometry                    |
 | nav_msgs/msg/Odometry                       | ignition::msgs::OdometryWithCovariance      |
 | rcl_interfaces/msg/ParameterValue           | ignition::msgs::Any                         |

ros_gz_bridge/include/ros_gz_bridge/convert.hpp

@@ -17,6 +17,7 @@
 
 #include <ros_gz_bridge/convert/actuator_msgs.hpp>
 #include <ros_gz_bridge/convert/geometry_msgs.hpp>
+#include <ros_gz_bridge/convert/gps_msgs.hpp>
 #include <ros_gz_bridge/convert/nav_msgs.hpp>
 #include <ros_gz_bridge/convert/ros_gz_interfaces.hpp>
 #include <ros_gz_bridge/convert/rosgraph_msgs.hpp>

ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp

@@ -0,0 +1,41 @@
+// Copyright 2022 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.
+
+#ifndef ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_
+#define ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_
+
+// Gazebo Msgs
+#include <gz/msgs/navsat.pb.h>
+
+// ROS 2 messages
+#include <gps_msgs/msg/gps_fix.hpp>
+
+#include <ros_gz_bridge/convert_decl.hpp>
+
+namespace ros_gz_bridge
+{
+template<>
+void
+convert_ros_to_gz(
+  const gps_msgs::msg::GPSFix & ros_msg,
+  gz::msgs::NavSat & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::NavSat & gz_msg,
+  gps_msgs::msg::GPSFix & ros_msg);
+}  // namespace ros_gz_bridge
+
+#endif  // ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_

ros_gz_bridge/package.xml

@@ -15,6 +15,7 @@
 
   <depend>actuator_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>gps_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -47,6 +47,9 @@
         Mapping('WrenchStamped', 'Wrench'),
         Mapping('Vector3', 'Vector3d'),
     ],
+    'gps_msgs': [
+        Mapping('GPSFix', 'NavSat'),
+    ],
     'nav_msgs': [
         Mapping('Odometry', 'Odometry'),
         Mapping('Odometry', 'OdometryWithCovariance'),

ros_gz_bridge/src/convert/gps_msgs.cpp

@@ -0,0 +1,63 @@
+// Copyright 2022 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.
+
+#include <cmath>
+
+#include "convert/utils.hpp"
+#include "ros_gz_bridge/convert/gps_msgs.hpp"
+
+namespace ros_gz_bridge
+{
+
+template<>
+void
+convert_ros_to_gz(
+  const gps_msgs::msg::GPSFix & ros_msg,
+  gz::msgs::NavSat & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  gz_msg.set_latitude_deg(ros_msg.latitude);
+  gz_msg.set_longitude_deg(ros_msg.longitude);
+  gz_msg.set_altitude(ros_msg.altitude);
+  gz_msg.set_frame_id(ros_msg.header.frame_id);
+
+  gz_msg.set_velocity_east(cos(ros_msg.track) * ros_msg.speed);
+  gz_msg.set_velocity_north(sin(ros_msg.track) * ros_msg.speed);
+  gz_msg.set_velocity_up(ros_msg.climb);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::NavSat & gz_msg,
+  gps_msgs::msg::GPSFix & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  ros_msg.header.frame_id = frame_id_gz_to_ros(gz_msg.frame_id());
+  ros_msg.latitude = gz_msg.latitude_deg();
+  ros_msg.longitude = gz_msg.longitude_deg();
+  ros_msg.altitude = gz_msg.altitude();
+
+  ros_msg.speed = sqrt(
+    gz_msg.velocity_east() * gz_msg.velocity_east() +
+    gz_msg.velocity_north() * gz_msg.velocity_north());
+  ros_msg.track = atan2(gz_msg.velocity_north(), gz_msg.velocity_east());
+  ros_msg.climb = gz_msg.velocity_up();
+
+  // position_covariance is not supported in Ignition::Msgs::NavSat.
+  ros_msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN;
+  ros_msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX;
+}
+
+}  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -521,6 +521,38 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::WrenchStamped> & _
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->wrench.force));
   compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->wrench.torque));
 }
+
+void createTestMsg(gps_msgs::msg::GPSFix & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+
+  _msg.header = header_msg;
+  _msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_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 = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN;
+}
+
+void compareTestMsg(const std::shared_ptr<gps_msgs::msg::GPSFix> & _msg)
+{
+  gps_msgs::msg::GPSFix 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(ros_gz_interfaces::msg::Light & _msg)
 {
   createTestMsg(_msg.header);

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -45,6 +45,7 @@
 #include <geometry_msgs/msg/vector3.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <gps_msgs/msg/gps_fix.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
@@ -343,6 +344,16 @@ void createTestMsg(geometry_msgs::msg::WrenchStamped & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::WrenchStamped> & _msg);
 
+/// gps_msgs
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gps_msgs::msg::GPSFix & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gps_msgs::msg::GPSFix> & _msg);
+
 /// tf2_msgs
 
 /// \brief Create a message used for testing.

ros_gz_sim_demos/launch/navsat_gpsfix.launch.py

@@ -0,0 +1,63 @@
+# Copyright 2022 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_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
+        launch_arguments={
+            'gz_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_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/navsat@gps_msgs/msg/[email protected]'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        gz_sim,
+        DeclareLaunchArgument('rqt', default_value='true',
+                              description='Open RQt.'),
+        bridge,
+        rqt
+    ])