Skip to content

Commit 0777767

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

File tree

8 files changed

+131
-2
lines changed

8 files changed

+131
-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);

0 commit comments

Comments
 (0)