Skip to content

Commit eb27eaa

Browse files
Port NavSat (#224) from ROS 1 to ROS 2 (#268)
Co-authored-by: Tyler Howell <76003804+TyHowellWork@users.noreply.github.com> Signed-off-by: Michael Carroll <michael@openrobotics.org>
1 parent fda6daa commit eb27eaa

File tree

11 files changed

+202
-2
lines changed

11 files changed

+202
-2
lines changed

ros_ign_bridge/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ service calls. Its support is limited to only the following message types:
5050
| sensor_msgs/msg/JointState | ignition::msgs::Model |
5151
| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan |
5252
| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer |
53+
| sensor_msgs/msg/NavSatFixed | ignition::msgs::NavSat |
5354
| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked |
5455
| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V |
5556
| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory |

ros_ign_bridge/include/ros_ign_bridge/convert/sensor_msgs.hpp

+14
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <sensor_msgs/msg/joint_state.hpp>
2424
#include <sensor_msgs/msg/laser_scan.hpp>
2525
#include <sensor_msgs/msg/magnetic_field.hpp>
26+
#include <sensor_msgs/msg/nav_sat_fix.hpp>
2627
#include <sensor_msgs/msg/point_cloud2.hpp>
2728

2829
// Ignition messages
@@ -34,6 +35,7 @@
3435
#include <ignition/msgs/laserscan.pb.h>
3536
#include <ignition/msgs/magnetometer.pb.h>
3637
#include <ignition/msgs/model.pb.h>
38+
#include <ignition/msgs/navsat.pb.h>
3739
#include <ignition/msgs/pointcloud_packed.pb.h>
3840

3941
#include <ros_ign_bridge/convert_decl.hpp>
@@ -126,6 +128,18 @@ convert_ign_to_ros(
126128
const ignition::msgs::Magnetometer & ign_msg,
127129
sensor_msgs::msg::MagneticField & ros_msg);
128130

131+
template<>
132+
void
133+
convert_ros_to_ign(
134+
const sensor_msgs::msg::NavSatFix & ros_msg,
135+
ignition::msgs::NavSat & ign_msg);
136+
137+
template<>
138+
void
139+
convert_ign_to_ros(
140+
const ignition::msgs::NavSat & ign_msg,
141+
sensor_msgs::msg::NavSatFix & ros_msg);
142+
129143
template<>
130144
void
131145
convert_ros_to_ign(

ros_ign_bridge/ros_ign_bridge/mappings.py

+3-2
Original file line numberDiff line numberDiff line change
@@ -58,15 +58,16 @@
5858
Mapping('Clock', 'Clock'),
5959
],
6060
'sensor_msgs': [
61+
Mapping('BatteryState', 'BatteryState'),
62+
Mapping('CameraInfo', 'CameraInfo'),
6163
Mapping('FluidPressure', 'FluidPressure'),
6264
Mapping('Image', 'Image'),
63-
Mapping('CameraInfo', 'CameraInfo'),
6465
Mapping('Imu', 'IMU'),
6566
Mapping('JointState', 'Model'),
6667
Mapping('LaserScan', 'LaserScan'),
6768
Mapping('MagneticField', 'Magnetometer'),
69+
Mapping('NavSatFix', 'NavSat'),
6870
Mapping('PointCloud2', 'PointCloudPacked'),
69-
Mapping('BatteryState', 'BatteryState'),
7071
],
7172
'std_msgs': [
7273
Mapping('Bool', 'Boolean'),

ros_ign_bridge/src/convert/sensor_msgs.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -416,6 +416,41 @@ convert_ign_to_ros(
416416
// magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer.
417417
}
418418

419+
template<>
420+
void
421+
convert_ros_to_ign(
422+
const sensor_msgs::msg::NavSatFix & ros_msg,
423+
ignition::msgs::NavSat & ign_msg)
424+
{
425+
convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
426+
ign_msg.set_latitude_deg(ros_msg.latitude);
427+
ign_msg.set_longitude_deg(ros_msg.longitude);
428+
ign_msg.set_altitude(ros_msg.altitude);
429+
ign_msg.set_frame_id(ros_msg.header.frame_id);
430+
431+
// Not supported in sensor_msgs::NavSatFix.
432+
ign_msg.set_velocity_east(0.0);
433+
ign_msg.set_velocity_north(0.0);
434+
ign_msg.set_velocity_up(0.0);
435+
}
436+
437+
template<>
438+
void
439+
convert_ign_to_ros(
440+
const ignition::msgs::NavSat & ign_msg,
441+
sensor_msgs::msg::NavSatFix & ros_msg)
442+
{
443+
convert_ign_to_ros(ign_msg.header(), ros_msg.header);
444+
ros_msg.header.frame_id = frame_id_ign_to_ros(ign_msg.frame_id());
445+
ros_msg.latitude = ign_msg.latitude_deg();
446+
ros_msg.longitude = ign_msg.longitude_deg();
447+
ros_msg.altitude = ign_msg.altitude();
448+
449+
// position_covariance is not supported in Ignition::Msgs::NavSat.
450+
ros_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
451+
ros_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
452+
}
453+
419454
template<>
420455
void
421456
convert_ros_to_ign(

ros_ign_bridge/test/utils/ign_test_msg.cpp

+29
Original file line numberDiff line numberDiff line change
@@ -831,6 +831,35 @@ void compareTestMsg(const std::shared_ptr<ignition::msgs::Magnetometer> & _msg)
831831
compareTestMsg(std::make_shared<ignition::msgs::Vector3d>(_msg->field_tesla()));
832832
}
833833

834+
void createTestMsg(ignition::msgs::NavSat & _msg)
835+
{
836+
ignition::msgs::Header header_msg;
837+
createTestMsg(header_msg);
838+
839+
_msg.mutable_header()->CopyFrom(header_msg);
840+
_msg.set_frame_id("frame_id_value");
841+
_msg.set_latitude_deg(0.00);
842+
_msg.set_longitude_deg(0.00);
843+
_msg.set_altitude(0.00);
844+
_msg.set_velocity_east(0.00);
845+
_msg.set_velocity_north(0.00);
846+
_msg.set_velocity_up(0.00);
847+
}
848+
849+
void compareTestMsg(const std::shared_ptr<ignition::msgs::NavSat> & _msg)
850+
{
851+
ignition::msgs::NavSat expected_msg;
852+
createTestMsg(expected_msg);
853+
854+
compareTestMsg(std::make_shared<ignition::msgs::Header>(_msg->header()));
855+
EXPECT_FLOAT_EQ(expected_msg.latitude_deg(), _msg->latitude_deg());
856+
EXPECT_FLOAT_EQ(expected_msg.longitude_deg(), _msg->longitude_deg());
857+
EXPECT_FLOAT_EQ(expected_msg.altitude(), _msg->altitude());
858+
EXPECT_FLOAT_EQ(expected_msg.velocity_east(), _msg->velocity_east());
859+
EXPECT_FLOAT_EQ(expected_msg.velocity_north(), _msg->velocity_north());
860+
EXPECT_FLOAT_EQ(expected_msg.velocity_up(), _msg->velocity_up());
861+
}
862+
834863
void createTestMsg(ignition::msgs::Actuators & _msg)
835864
{
836865
ignition::msgs::Header header_msg;

ros_ign_bridge/test/utils/ign_test_msg.hpp

+9
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
#include <ignition/msgs/light.pb.h>
4747
#include <ignition/msgs/magnetometer.pb.h>
4848
#include <ignition/msgs/model.pb.h>
49+
#include <ignition/msgs/navsat.pb.h>
4950
#include <ignition/msgs/odometry.pb.h>
5051
#include <ignition/msgs/param.pb.h>
5152
#include <ignition/msgs/pointcloud_packed.pb.h>
@@ -325,6 +326,14 @@ void createTestMsg(ignition::msgs::Magnetometer & _msg);
325326
/// \param[in] _msg The message to compare.
326327
void compareTestMsg(const std::shared_ptr<ignition::msgs::Magnetometer> & _msg);
327328

329+
/// \brief Create a message used for testing.
330+
/// \param[out] _msg The message populated.
331+
void createTestMsg(ignition::msgs::NavSat & _msg);
332+
333+
/// \brief Compare a message with the populated for testing.
334+
/// \param[in] _msg The message to compare.
335+
void compareTestMsg(const std::shared_ptr<ignition::msgs::NavSat> & _msg);
336+
328337
/// \brief Create a message used for testing.
329338
/// \param[out] _msg The message populated.
330339
void createTestMsg(ignition::msgs::Actuators & _msg);

ros_ign_bridge/test/utils/ros_test_msg.cpp

+31
Original file line numberDiff line numberDiff line change
@@ -919,6 +919,37 @@ void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::MagneticField> & _ms
919919
compareTestMsg(std::make_shared<geometry_msgs::msg::Vector3>(_msg->magnetic_field));
920920
}
921921

922+
void createTestMsg(sensor_msgs::msg::NavSatFix & _msg)
923+
{
924+
std_msgs::msg::Header header_msg;
925+
createTestMsg(header_msg);
926+
927+
_msg.header = header_msg;
928+
_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
929+
_msg.latitude = 0.00;
930+
_msg.longitude = 0.00;
931+
_msg.altitude = 0.00;
932+
_msg.position_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
933+
_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
934+
}
935+
936+
void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::NavSatFix> & _msg)
937+
{
938+
sensor_msgs::msg::NavSatFix expected_msg;
939+
createTestMsg(expected_msg);
940+
941+
compareTestMsg(_msg->header);
942+
EXPECT_EQ(expected_msg.status, _msg->status);
943+
EXPECT_FLOAT_EQ(expected_msg.latitude, _msg->latitude);
944+
EXPECT_FLOAT_EQ(expected_msg.longitude, _msg->longitude);
945+
EXPECT_FLOAT_EQ(expected_msg.altitude, _msg->altitude);
946+
EXPECT_EQ(expected_msg.position_covariance_type, _msg->position_covariance_type);
947+
948+
for (auto i = 0u; i < 9; ++i) {
949+
EXPECT_FLOAT_EQ(0, _msg->position_covariance[i]);
950+
}
951+
}
952+
922953
void createTestMsg(sensor_msgs::msg::PointCloud2 & _msg)
923954
{
924955
createTestMsg(_msg.header);

ros_ign_bridge/test/utils/ros_test_msg.hpp

+9
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@
5858
#include <sensor_msgs/msg/joint_state.hpp>
5959
#include <sensor_msgs/msg/laser_scan.hpp>
6060
#include <sensor_msgs/msg/magnetic_field.hpp>
61+
#include <sensor_msgs/msg/nav_sat_fix.hpp>
6162
#include <sensor_msgs/msg/point_cloud2.hpp>
6263
#include <sensor_msgs/msg/point_field.hpp>
6364
#include <tf2_msgs/msg/tf_message.hpp>
@@ -423,6 +424,14 @@ void createTestMsg(sensor_msgs::msg::MagneticField & _msg);
423424
/// \param[in] _msg The message to compare.
424425
void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::MagneticField> & _msg);
425426

427+
/// \brief Create a message used for testing.
428+
/// \param[out] _msg The message populated.
429+
void createTestMsg(sensor_msgs::msg::NavSatFix & _msg);
430+
431+
/// \brief Compare a message with the populated for testing.
432+
/// \param[in] _msg The message to compare.
433+
void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::NavSatFix> & _msg);
434+
426435
/// \brief Create a message used for testing.
427436
/// \param[out] _msg The message populated.
428437
void createTestMsg(sensor_msgs::msg::PointCloud2 & _msg);

ros_ign_gazebo_demos/README.md

+8
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,14 @@ Publishes magnetic field readings.
122122

123123
![](images/magnetometer_demo.png)
124124

125+
## GNSS
126+
127+
Publishes satellite navigation readings, only available in Fortress on.
128+
129+
ros2 launch ros_ign_gazebo_demos navsat.launch.py
130+
131+
![](images/navsat_demo.png)
132+
125133
## RGBD camera
126134

127135
RGBD camera data can be obtained as:
188 KB
Loading
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from ament_index_python.packages import get_package_share_directory
18+
19+
from launch import LaunchDescription
20+
from launch.actions import DeclareLaunchArgument
21+
from launch.actions import IncludeLaunchDescription
22+
from launch.conditions import IfCondition
23+
from launch.launch_description_sources import PythonLaunchDescriptionSource
24+
from launch.substitutions import LaunchConfiguration
25+
26+
from launch_ros.actions import Node
27+
28+
29+
def generate_launch_description():
30+
31+
pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
32+
33+
ign_gazebo = IncludeLaunchDescription(
34+
PythonLaunchDescriptionSource(
35+
os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
36+
launch_arguments={
37+
'ign_args': '-v 4 -r spherical_coordinates.sdf'
38+
}.items(),
39+
)
40+
41+
# RQt
42+
rqt = Node(
43+
package='rqt_topic',
44+
executable='rqt_topic',
45+
arguments=['-t'],
46+
condition=IfCondition(LaunchConfiguration('rqt'))
47+
)
48+
49+
# Bridge
50+
bridge = Node(
51+
package='ros_ign_bridge',
52+
executable='parameter_bridge',
53+
arguments=['/navsat@sensor_msgs/msg/NavSatFix@ignition.msgs.NavSat'],
54+
output='screen'
55+
)
56+
57+
return LaunchDescription([
58+
ign_gazebo,
59+
DeclareLaunchArgument('rqt', default_value='true',
60+
description='Open RQt.'),
61+
bridge,
62+
rqt
63+
])

0 commit comments

Comments
 (0)