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 1261536a..0715ad8b 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -31,6 +31,7 @@ The following message types can be bridged for topics: | geometry_msgs/msg/Twist | ignition::msgs::Twist | | geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | | geometry_msgs/msg/TwistWithCovariance| 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 ae19956f..16e16f83 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -16,6 +16,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 c37a0a1a..321cf502 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -46,6 +46,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..cf2774d6 --- /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 gz::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/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp index cddc4f00..b885b738 100644 --- a/ros_gz_bridge/src/convert/sensor_msgs.cpp +++ b/ros_gz_bridge/src/convert/sensor_msgs.cpp @@ -480,7 +480,7 @@ convert_gz_to_ros( ros_msg.longitude = gz_msg.longitude_deg(); ros_msg.altitude = gz_msg.altitude(); - // position_covariance is not supported in Ignition::Msgs::NavSat. + // position_covariance is not supported in gz::msgs::NavSat. ros_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; ros_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; } diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index a6a0f648..634c8ebd 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -503,6 +503,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 85a91183..e97fa82b 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -334,6 +335,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 + ])