diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index 16d633d2..80beae71 100644 --- a/ros_ign_bridge/README.md +++ b/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 | diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp index 7eab5e2e..683c6bee 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include // Ignition messages @@ -34,6 +35,7 @@ #include #include #include +#include #include #include @@ -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( diff --git a/ros_ign_bridge/ros_ign_bridge/mappings.py b/ros_ign_bridge/ros_ign_bridge/mappings.py index 28b532de..fa0953f6 100644 --- a/ros_ign_bridge/ros_ign_bridge/mappings.py +++ b/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'), diff --git a/ros_ign_bridge/src/convert/sensor_msgs.cpp b/ros_ign_bridge/src/convert/sensor_msgs.cpp index 3da2265d..23f4fb6a 100644 --- a/ros_ign_bridge/src/convert/sensor_msgs.cpp +++ b/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( diff --git a/ros_ign_bridge/test/utils/ign_test_msg.cpp b/ros_ign_bridge/test/utils/ign_test_msg.cpp index 02b7bbd5..511fe1c2 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.cpp @@ -831,6 +831,35 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_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 & _msg) +{ + ignition::msgs::NavSat expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_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; diff --git a/ros_ign_bridge/test/utils/ign_test_msg.hpp b/ros_ign_bridge/test/utils/ign_test_msg.hpp index e5ddaf12..b190404a 100644 --- a/ros_ign_bridge/test/utils/ign_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ign_test_msg.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -325,6 +326,14 @@ void createTestMsg(ignition::msgs::Magnetometer & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::Actuators & _msg); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.cpp b/ros_ign_bridge/test/utils/ros_test_msg.cpp index 8965a533..05fbb236 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.cpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.cpp @@ -919,6 +919,37 @@ void compareTestMsg(const std::shared_ptr & _ms compareTestMsg(std::make_shared(_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 & _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); diff --git a/ros_ign_bridge/test/utils/ros_test_msg.hpp b/ros_ign_bridge/test/utils/ros_test_msg.hpp index 9569c9fc..0396a566 100644 --- a/ros_ign_bridge/test/utils/ros_test_msg.hpp +++ b/ros_ign_bridge/test/utils/ros_test_msg.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -423,6 +424,14 @@ void createTestMsg(sensor_msgs::msg::MagneticField & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(sensor_msgs::msg::PointCloud2 & _msg); diff --git a/ros_ign_gazebo_demos/README.md b/ros_ign_gazebo_demos/README.md index 238937a2..ab7c2de3 100644 --- a/ros_ign_gazebo_demos/README.md +++ b/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: diff --git a/ros_ign_gazebo_demos/images/navsat_demo.png b/ros_ign_gazebo_demos/images/navsat_demo.png new file mode 100644 index 00000000..1d6c3428 Binary files /dev/null and b/ros_ign_gazebo_demos/images/navsat_demo.png differ diff --git a/ros_ign_gazebo_demos/launch/navsat.launch.py b/ros_ign_gazebo_demos/launch/navsat.launch.py new file mode 100644 index 00000000..1099de0e --- /dev/null +++ b/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/NavSatFix@ignition.msgs.NavSat'], + output='screen' + ) + + return LaunchDescription([ + ign_gazebo, + DeclareLaunchArgument('rqt', default_value='true', + description='Open RQt.'), + bridge, + rqt + ])