From a348c07527c54f387a576413a3e3d6d7431ff45b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 16 Apr 2024 18:35:29 +0200 Subject: [PATCH] [backport Humble] Create bridge for GPSFix msg (#316) (#538) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Vincent Rousseau Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Rousseau Vincent --- ros_gz_bridge/CMakeLists.txt | 1 + ros_gz_bridge/README.md | 1 + .../include/ros_gz_bridge/convert.hpp | 1 + .../ros_gz_bridge/convert/gps_msgs.hpp | 41 ++++++++++++ ros_gz_bridge/package.xml | 1 + ros_gz_bridge/ros_gz_bridge/mappings.py | 3 + ros_gz_bridge/src/convert/gps_msgs.cpp | 63 +++++++++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 32 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 11 ++++ .../launch/navsat_gpsfix.launch.py | 63 +++++++++++++++++++ 10 files changed, 217 insertions(+) create mode 100644 ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp create mode 100644 ros_gz_bridge/src/convert/gps_msgs.cpp create mode 100644 ros_gz_sim_demos/launch/navsat_gpsfix.launch.py diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 7df2399b..8f4cc27d 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/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 diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 242d92b2..f89837c2 100644 --- a/ros_gz_bridge/README.md +++ b/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 | diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert.hpp index 98ead1eb..e25a82c8 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp new file mode 100644 index 00000000..d6cf194e --- /dev/null +++ b/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 + +// ROS 2 messages +#include + +#include + +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_ diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index 811955d2..440d7c87 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -15,6 +15,7 @@ actuator_msgs geometry_msgs + gps_msgs nav_msgs rclcpp rclcpp_components diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index ddec0f68..d17fb56d 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), diff --git a/ros_gz_bridge/src/convert/gps_msgs.cpp b/ros_gz_bridge/src/convert/gps_msgs.cpp new file mode 100644 index 00000000..de806239 --- /dev/null +++ b/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 + +#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 diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index c6f085e2..76e80d29 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -521,6 +521,38 @@ void compareTestMsg(const std::shared_ptr & _ compareTestMsg(std::make_shared(_msg->wrench.force)); compareTestMsg(std::make_shared(_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 & _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); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 5a39c4ab..6bc395cb 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -343,6 +344,16 @@ void createTestMsg(geometry_msgs::msg::WrenchStamped & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// tf2_msgs /// \brief Create a message used for testing. diff --git a/ros_gz_sim_demos/launch/navsat_gpsfix.launch.py b/ros_gz_sim_demos/launch/navsat_gpsfix.launch.py new file mode 100644 index 00000000..83adcdcc --- /dev/null +++ b/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/GPSFix@gz.msgs.NavSat'], + output='screen' + ) + + return LaunchDescription([ + gz_sim, + DeclareLaunchArgument('rqt', default_value='true', + description='Open RQt.'), + bridge, + rqt + ])