Skip to content

Commit 4c1c819

Browse files
TyHowellWorkTyHowellWork
authored
Add support for bridging NavSat (#224)
Signed-off-by: Tyler Howell <76003804+TyHowellWork@users.noreply.github.com>
1 parent ced2293 commit 4c1c819

15 files changed

+243
-0
lines changed

‎ros_ign_bridge/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ service calls. Its support is limited to only the following message types:
3737
| sensor_msgs/JointState | ignition::msgs::Model |
3838
| sensor_msgs/LaserScan | ignition::msgs::LaserScan |
3939
| sensor_msgs/MagneticField | ignition::msgs::Magnetometer |
40+
| sensor_msgs/NavSatFix | ignition::msgs::NavSat |
4041
| sensor_msgs/PointCloud2 | ignition::msgs::PointCloudPacked |
4142
| tf_msgs/TFMessage | ignition::msgs::Pose_V |
4243
| visualization_msgs/Marker | ignition::msgs::Marker |

‎ros_ign_bridge/include/ros_ign_bridge/convert.hpp

+13
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
#include <sensor_msgs/JointState.h>
3838
#include <sensor_msgs/LaserScan.h>
3939
#include <sensor_msgs/MagneticField.h>
40+
#include <sensor_msgs/NavSatFix.h>
4041
#include <sensor_msgs/PointCloud2.h>
4142
#include <std_msgs/Bool.h>
4243
#include <std_msgs/ColorRGBA.h>
@@ -412,6 +413,18 @@ convert_ign_to_ros(
412413
const ignition::msgs::Magnetometer & ign_msg,
413414
sensor_msgs::MagneticField & ros_msg);
414415

416+
template<>
417+
void
418+
convert_ros_to_ign(
419+
const sensor_msgs::NavSatFix & ros_msg,
420+
ignition::msgs::NavSat & ign_msg);
421+
422+
template<>
423+
void
424+
convert_ign_to_ros(
425+
const ignition::msgs::NavSat & ign_msg,
426+
sensor_msgs::NavSatFix & ros_msg);
427+
415428
template<>
416429
void
417430
convert_ros_to_ign(

‎ros_ign_bridge/src/convert.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -1141,6 +1141,41 @@ convert_ign_to_ros(
11411141
// magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer.
11421142
}
11431143

1144+
template<>
1145+
void
1146+
convert_ros_to_ign(
1147+
const sensor_msgs::NavSatFix & ros_msg,
1148+
ignition::msgs::NavSat & ign_msg)
1149+
{
1150+
convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1151+
ign_msg.set_latitude_deg(ros_msg.latitude);
1152+
ign_msg.set_longitude_deg(ros_msg.longitude);
1153+
ign_msg.set_altitude(ros_msg.altitude);
1154+
ign_msg.set_frame_id(ros_msg.header.frame_id);
1155+
1156+
// Not supported in sensor_msgs::NavSatFix.
1157+
ign_msg.set_velocity_east(0.0);
1158+
ign_msg.set_velocity_north(0.0);
1159+
ign_msg.set_velocity_up(0.0);
1160+
}
1161+
1162+
template<>
1163+
void
1164+
convert_ign_to_ros(
1165+
const ignition::msgs::NavSat & ign_msg,
1166+
sensor_msgs::NavSatFix & ros_msg)
1167+
{
1168+
convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1169+
ros_msg.header.frame_id = frame_id_ign_to_ros(ign_msg.frame_id());
1170+
ros_msg.latitude = ign_msg.latitude_deg();
1171+
ros_msg.longitude = ign_msg.longitude_deg();
1172+
ros_msg.altitude = ign_msg.altitude();
1173+
1174+
// position_covariance is not supported in Ignition::Msgs::NavSat.
1175+
ros_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
1176+
ros_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
1177+
}
1178+
11441179
template<>
11451180
void
11461181
convert_ros_to_ign(

‎ros_ign_bridge/src/factories.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -346,6 +346,17 @@ get_factory_impl(
346346
>
347347
>("sensor_msgs/Magnetometer", ign_type_name);
348348
}
349+
if (
350+
(ros_type_name == "sensor_msgs/NavSatFix" || ros_type_name == "") &&
351+
ign_type_name == "ignition.msgs.NavSat")
352+
{
353+
return std::make_shared<
354+
Factory<
355+
sensor_msgs::NavSatFix,
356+
ignition::msgs::NavSat
357+
>
358+
>("sensor_msgs/NavSatFix", ign_type_name);
359+
}
349360
if (
350361
(ros_type_name == "sensor_msgs/PointCloud2" || ros_type_name == "") &&
351362
ign_type_name == "ignition.msgs.PointCloudPacked")
@@ -1109,6 +1120,30 @@ Factory<
11091120
ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
11101121
}
11111122

1123+
template<>
1124+
void
1125+
Factory<
1126+
sensor_msgs::NavSatFix,
1127+
ignition::msgs::NavSat
1128+
>::convert_ros_to_ign(
1129+
const sensor_msgs::NavSatFix & ros_msg,
1130+
ignition::msgs::NavSat & ign_msg)
1131+
{
1132+
ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
1133+
}
1134+
1135+
template<>
1136+
void
1137+
Factory<
1138+
sensor_msgs::NavSatFix,
1139+
ignition::msgs::NavSat
1140+
>::convert_ign_to_ros(
1141+
const ignition::msgs::NavSat & ign_msg,
1142+
sensor_msgs::NavSatFix & ros_msg)
1143+
{
1144+
ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
1145+
}
1146+
11121147
template<>
11131148
void
11141149
Factory<

‎ros_ign_bridge/src/factories.hpp

+20
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
#include <sensor_msgs/JointState.h>
3838
#include <sensor_msgs/LaserScan.h>
3939
#include <sensor_msgs/MagneticField.h>
40+
#include <sensor_msgs/NavSatFix.h>
4041
#include <sensor_msgs/PointCloud2.h>
4142
#include <std_msgs/Bool.h>
4243
#include <std_msgs/ColorRGBA.h>
@@ -594,6 +595,25 @@ Factory<
594595
const ignition::msgs::Magnetometer & ign_msg,
595596
sensor_msgs::MagneticField & ros_msg);
596597

598+
template<>
599+
void
600+
Factory<
601+
sensor_msgs::NavSatFix,
602+
ignition::msgs::NavSat
603+
>::convert_ros_to_ign(
604+
const sensor_msgs::NavSatFix & ros_msg,
605+
ignition::msgs::NavSat & ign_msg);
606+
607+
template<>
608+
void
609+
Factory<
610+
sensor_msgs::NavSatFix,
611+
ignition::msgs::NavSat
612+
>::convert_ign_to_ros(
613+
const ignition::msgs::NavSat & ign_msg,
614+
sensor_msgs::NavSatFix & ros_msg);
615+
616+
597617
template<>
598618
void
599619
Factory<

‎ros_ign_bridge/test/launch/test_ign_subscriber.launch

+1
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
/imu@sensor_msgs/Imu@ignition.msgs.IMU
2929
/laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan
3030
/magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer
31+
/navsat@sensor_msgs/NavSatFix@ignition.msgs.NavSat
3132
/actuators@mav_msgs/Actuators@ignition.msgs.Actuators
3233
/map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid
3334
/odometry@nav_msgs/Odometry@ignition.msgs.Odometry

‎ros_ign_bridge/test/launch/test_ros_subscriber.launch

+1
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
/imu@sensor_msgs/Imu@ignition.msgs.IMU
2929
/laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan
3030
/magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer
31+
/navsat@sensor_msgs/NavSatFix@ignition.msgs.NavSat
3132
/actuators@mav_msgs/Actuators@ignition.msgs.Actuators
3233
/map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid
3334
/odometry@nav_msgs/Odometry@ignition.msgs.Odometry

‎ros_ign_bridge/test/publishers/ign_publisher.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,11 @@ int main(int /*argc*/, char **/*argv*/)
171171
ignition::msgs::Magnetometer magnetometer_msg;
172172
ros_ign_bridge::testing::createTestMsg(magnetometer_msg);
173173

174+
// ignition::msgs::NavSat.
175+
auto navsat_pub = node.Advertise<ignition::msgs::NavSat>("navsat");
176+
ignition::msgs::NavSat navsat_msg;
177+
ros_ign_bridge::testing::createTestMsg(navsat_msg);
178+
174179
// ignition::msgs::Actuators.
175180
auto actuators_pub = node.Advertise<ignition::msgs::Actuators>("actuators");
176181
ignition::msgs::Actuators actuators_msg;
@@ -242,6 +247,7 @@ int main(int /*argc*/, char **/*argv*/)
242247
imu_pub.Publish(imu_msg);
243248
laserscan_pub.Publish(laserscan_msg);
244249
magnetic_pub.Publish(magnetometer_msg);
250+
navsat_pub.Publish(navsat_msg);
245251
actuators_pub.Publish(actuators_msg);
246252
map_pub.Publish(map_msg);
247253
odometry_pub.Publish(odometry_msg);

‎ros_ign_bridge/test/publishers/ros_publisher.cpp

+8
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
#include <sensor_msgs/JointState.h>
4343
#include <sensor_msgs/LaserScan.h>
4444
#include <sensor_msgs/MagneticField.h>
45+
#include <sensor_msgs/NavSatFix.h>
4546
#include <sensor_msgs/PointCloud2.h>
4647
#include <tf2_msgs/TFMessage.h>
4748
#include <visualization_msgs/Marker.h>
@@ -220,6 +221,12 @@ int main(int argc, char ** argv)
220221
sensor_msgs::MagneticField magnetic_msg;
221222
ros_ign_bridge::testing::createTestMsg(magnetic_msg);
222223

224+
// sensor_msgs::NavSatFix.
225+
ros::Publisher navsat_pub =
226+
n.advertise<sensor_msgs::NavSatFix>("navsat", 1000);
227+
sensor_msgs::NavSatFix navsat_msg;
228+
ros_ign_bridge::testing::createTestMsg(navsat_msg);
229+
223230
// sensor_msgs::PointCloud2.
224231
ros::Publisher pointcloud2_pub =
225232
n.advertise<sensor_msgs::PointCloud2>("pointcloud2", 1000);
@@ -275,6 +282,7 @@ int main(int argc, char ** argv)
275282
imu_pub.publish(imu_msg);
276283
laserscan_pub.publish(laserscan_msg);
277284
magnetic_pub.publish(magnetic_msg);
285+
navsat_pub.publish(navsat_msg);
278286
joint_states_pub.publish(joint_states_msg);
279287
pointcloud2_pub.publish(pointcloud2_msg);
280288
battery_state_pub.publish(battery_state_msg);

‎ros_ign_bridge/test/subscribers/ign_subscriber.cpp

+12
Original file line numberDiff line numberDiff line change
@@ -335,6 +335,18 @@ TEST(IgnSubscriberTest, Magnetometer)
335335
EXPECT_TRUE(client.callbackExecuted);
336336
}
337337

338+
/////////////////////////////////////////////////
339+
TEST(IgnSubscriberTest, NavSat)
340+
{
341+
MyTestClass<ignition::msgs::NavSat> client("navsat");
342+
343+
using namespace std::chrono_literals;
344+
ros_ign_bridge::testing::waitUntilBoolVar(
345+
client.callbackExecuted, 10ms, 200);
346+
347+
EXPECT_TRUE(client.callbackExecuted);
348+
}
349+
338350
/////////////////////////////////////////////////
339351
TEST(IgnSubscriberTest, Actuators)
340352
{

‎ros_ign_bridge/test/subscribers/ros_subscriber.cpp

+12
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@
4444
#include <sensor_msgs/JointState.h>
4545
#include <sensor_msgs/LaserScan.h>
4646
#include <sensor_msgs/MagneticField.h>
47+
#include <sensor_msgs/NavSatFix.h>
4748
#include <sensor_msgs/PointCloud2.h>
4849
#include <visualization_msgs/Marker.h>
4950
#include <visualization_msgs/MarkerArray.h>
@@ -391,6 +392,17 @@ TEST(ROSSubscriberTest, MagneticField)
391392
EXPECT_TRUE(client.callbackExecuted);
392393
}
393394

395+
TEST(ROSSubscriberTest, NavSatFix)
396+
{
397+
MyTestClass<sensor_msgs::NavSatFix> client("navsat");
398+
399+
using namespace std::chrono_literals;
400+
ros_ign_bridge::testing::waitUntilBoolVarAndSpin(
401+
client.callbackExecuted, 10ms, 200);
402+
403+
EXPECT_TRUE(client.callbackExecuted);
404+
}
405+
394406
/////////////////////////////////////////////////
395407
TEST(ROSSubscriberTest, Actuators)
396408
{

‎ros_ign_bridge/test/test_utils.h

+70
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
#include <sensor_msgs/JointState.h>
4949
#include <sensor_msgs/LaserScan.h>
5050
#include <sensor_msgs/MagneticField.h>
51+
#include <sensor_msgs/NavSatFix.h>
5152
#include <sensor_msgs/PointCloud2.h>
5253
#include <sensor_msgs/PointField.h>
5354
#include <tf2_msgs/TFMessage.h>
@@ -832,6 +833,42 @@ namespace testing
832833
EXPECT_FLOAT_EQ(0, _msg.magnetic_field_covariance[i]);
833834
}
834835

836+
/// \brief Create a message used for testing.
837+
/// \param[out] _msg The message populated.
838+
void createTestMsg(sensor_msgs::NavSatFix &_msg)
839+
{
840+
std_msgs::Header header_msg;
841+
createTestMsg(header_msg);
842+
843+
_msg.header = header_msg;
844+
_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
845+
_msg.latitude = 0.00;
846+
_msg.longitude = 0.00;
847+
_msg.altitude = 0.00;
848+
_msg.position_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
849+
_msg.position_covariance_type =
850+
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
851+
}
852+
853+
/// \brief Compare a message with the populated for testing.
854+
/// \param[in] _msg The message to compare.
855+
void compareTestMsg(const sensor_msgs::NavSatFix &_msg)
856+
{
857+
sensor_msgs::NavSatFix expected_msg;
858+
createTestMsg(expected_msg);
859+
860+
compareTestMsg(_msg.header);
861+
EXPECT_EQ(expected_msg.status, _msg.status);
862+
EXPECT_FLOAT_EQ(expected_msg.latitude, _msg.latitude);
863+
EXPECT_FLOAT_EQ(expected_msg.longitude, _msg.longitude);
864+
EXPECT_FLOAT_EQ(expected_msg.altitude, _msg.altitude);
865+
EXPECT_EQ(expected_msg.position_covariance_type,
866+
_msg.position_covariance_type);
867+
868+
for (auto i = 0u; i < 9; ++i)
869+
EXPECT_FLOAT_EQ(0, _msg.position_covariance[i]);
870+
}
871+
835872
/// \brief Create a message used for testing.
836873
/// \param[out] _msg The message populated.
837874
void createTestMsg(sensor_msgs::PointCloud2 &_msg)
@@ -1633,6 +1670,39 @@ namespace testing
16331670
compareTestMsg(_msg.field_tesla());
16341671
}
16351672

1673+
/// \brief Create a message used for testing.
1674+
/// \param[out] _msg The message populated.
1675+
void createTestMsg(ignition::msgs::NavSat &_msg)
1676+
{
1677+
ignition::msgs::Header header_msg;
1678+
createTestMsg(header_msg);
1679+
1680+
_msg.mutable_header()->CopyFrom(header_msg);
1681+
_msg.set_frame_id("frame_id_value");
1682+
_msg.set_latitude_deg(0.00);
1683+
_msg.set_longitude_deg(0.00);
1684+
_msg.set_altitude(0.00);
1685+
_msg.set_velocity_east(0.00);
1686+
_msg.set_velocity_north(0.00);
1687+
_msg.set_velocity_up(0.00);
1688+
}
1689+
1690+
/// \brief Compare a message with the populated for testing.
1691+
/// \param[in] _msg The message to compare.
1692+
void compareTestMsg(const ignition::msgs::NavSat &_msg)
1693+
{
1694+
ignition::msgs::NavSat expected_msg;
1695+
createTestMsg(expected_msg);
1696+
1697+
compareTestMsg(_msg.header());
1698+
EXPECT_FLOAT_EQ(expected_msg.latitude_deg(), _msg.latitude_deg());
1699+
EXPECT_FLOAT_EQ(expected_msg.longitude_deg(), _msg.longitude_deg());
1700+
EXPECT_FLOAT_EQ(expected_msg.altitude(), _msg.altitude());
1701+
EXPECT_FLOAT_EQ(expected_msg.velocity_east(), _msg.velocity_east());
1702+
EXPECT_FLOAT_EQ(expected_msg.velocity_north(), _msg.velocity_north());
1703+
EXPECT_FLOAT_EQ(expected_msg.velocity_up(), _msg.velocity_up());
1704+
}
1705+
16361706
/// \brief Create a message used for testing.
16371707
/// \param[out] _msg The message populated.
16381708
void createTestMsg(ignition::msgs::Actuators &_msg)

‎ros_ign_gazebo_demos/README.md

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

9393
![](images/magnetometer_demo.png)
9494

95+
## GNSS
96+
97+
Publishes satellite navigation readings, only available in Ignition releases from Fortress on.
98+
99+
roslaunch ros_ign_gazebo_demos navsat.launch
100+
101+
![](images/navsat_demo.png)
102+
95103
## RGBD camera
96104

97105
RGBD camera data can be obtained as:

‎ros_ign_gazebo_demos/images/navsat_demo.png

188 KB
Loading

‎ros_ign_gazebo_demos/launch/navsat.launch

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
4+
<include file="$(find ros_ign_gazebo)/launch/ign_gazebo.launch">
5+
<arg name="ign_args" value="-r -v 3 spherical_coordinates.sdf"/>
6+
</include>
7+
8+
<node
9+
pkg="ros_ign_bridge"
10+
type="parameter_bridge"
11+
name="$(anon ros_ign_bridge)"
12+
output="screen"
13+
args="/navsat@sensor_msgs/NavSatFix@ignition.msgs.NavSat">
14+
</node>
15+
16+
<node
17+
type="rqt_topic"
18+
name="rqt_topic"
19+
pkg="rqt_topic"
20+
args="-t" />
21+
</launch>

0 commit comments

Comments
 (0)