diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index fc94ee12..fb1600e2 100644 --- a/ros_ign_bridge/README.md +++ b/ros_ign_bridge/README.md @@ -37,6 +37,7 @@ service calls. Its support is limited to only the following message types: | sensor_msgs/JointState | ignition::msgs::Model | | sensor_msgs/LaserScan | ignition::msgs::LaserScan | | sensor_msgs/MagneticField | ignition::msgs::Magnetometer | +| sensor_msgs/NavSatFix | ignition::msgs::NavSat | | sensor_msgs/PointCloud2 | ignition::msgs::PointCloudPacked | | tf_msgs/TFMessage | ignition::msgs::Pose_V | | visualization_msgs/Marker | ignition::msgs::Marker | diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert.hpp index a1903aba..1720c7a4 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -412,6 +413,18 @@ convert_ign_to_ros( const ignition::msgs::Magnetometer & ign_msg, sensor_msgs::MagneticField & ros_msg); +template<> +void +convert_ros_to_ign( + const sensor_msgs::NavSatFix & ros_msg, + ignition::msgs::NavSat & ign_msg); + +template<> +void +convert_ign_to_ros( + const ignition::msgs::NavSat & ign_msg, + sensor_msgs::NavSatFix & ros_msg); + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/convert.cpp b/ros_ign_bridge/src/convert.cpp index 5299ab41..3b41e654 100644 --- a/ros_ign_bridge/src/convert.cpp +++ b/ros_ign_bridge/src/convert.cpp @@ -1141,6 +1141,41 @@ convert_ign_to_ros( // magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer. } +template<> +void +convert_ros_to_ign( + const sensor_msgs::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::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::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + ros_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; +} + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/factories.cpp b/ros_ign_bridge/src/factories.cpp index a3d95127..a8a3d14e 100644 --- a/ros_ign_bridge/src/factories.cpp +++ b/ros_ign_bridge/src/factories.cpp @@ -346,6 +346,17 @@ get_factory_impl( > >("sensor_msgs/Magnetometer", ign_type_name); } + if ( + (ros_type_name == "sensor_msgs/NavSatFix" || ros_type_name == "") && + ign_type_name == "ignition.msgs.NavSat") + { + return std::make_shared< + Factory< + sensor_msgs::NavSatFix, + ignition::msgs::NavSat + > + >("sensor_msgs/NavSatFix", ign_type_name); + } if ( (ros_type_name == "sensor_msgs/PointCloud2" || ros_type_name == "") && ign_type_name == "ignition.msgs.PointCloudPacked") @@ -1109,6 +1120,30 @@ Factory< ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); } +template<> +void +Factory< + sensor_msgs::NavSatFix, + ignition::msgs::NavSat +>::convert_ros_to_ign( + const sensor_msgs::NavSatFix & ros_msg, + ignition::msgs::NavSat & ign_msg) +{ + ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg); +} + +template<> +void +Factory< + sensor_msgs::NavSatFix, + ignition::msgs::NavSat +>::convert_ign_to_ros( + const ignition::msgs::NavSat & ign_msg, + sensor_msgs::NavSatFix & ros_msg) +{ + ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg); +} + template<> void Factory< diff --git a/ros_ign_bridge/src/factories.hpp b/ros_ign_bridge/src/factories.hpp index 303a2168..2b5a6283 100644 --- a/ros_ign_bridge/src/factories.hpp +++ b/ros_ign_bridge/src/factories.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -594,6 +595,24 @@ Factory< const ignition::msgs::Magnetometer & ign_msg, sensor_msgs::MagneticField & ros_msg); +template<> +void +Factory< + sensor_msgs::NavSatFix, + ignition::msgs::NavSat +>::convert_ros_to_ign( + const sensor_msgs::NavSatFix & ros_msg, + ignition::msgs::NavSat & ign_msg); + +template<> +void +Factory< + sensor_msgs::NavSatFix, + ignition::msgs::NavSat +>::convert_ign_to_ros( + const ignition::msgs::NavSat & ign_msg, + sensor_msgs::NavSatFix & ros_msg); + template<> void Factory< diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch b/ros_ign_bridge/test/launch/test_ign_subscriber.launch index 961399d1..84b1543b 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch @@ -28,6 +28,7 @@ /imu@sensor_msgs/Imu@ignition.msgs.IMU /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer + /navsat@sensor_msgs/NavSatFix@ignition.msgs.NavSat /actuators@mav_msgs/Actuators@ignition.msgs.Actuators /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid /odometry@nav_msgs/Odometry@ignition.msgs.Odometry diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch b/ros_ign_bridge/test/launch/test_ros_subscriber.launch index 3d2be3b5..41e422bf 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch @@ -28,6 +28,7 @@ /imu@sensor_msgs/Imu@ignition.msgs.IMU /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer + /navsat@sensor_msgs/NavSatFix@ignition.msgs.NavSat /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid /odometry@nav_msgs/Odometry@ignition.msgs.Odometry /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index 24205a33..e86b037b 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -171,6 +171,11 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Magnetometer magnetometer_msg; ros_ign_bridge::testing::createTestMsg(magnetometer_msg); + // ignition::msgs::NavSat. + auto navsat_pub = node.Advertise("navsat"); + ignition::msgs::NavSat navsat_msg; + ros_ign_bridge::testing::createTestMsg(navsat_msg); + // ignition::msgs::Actuators. auto actuators_pub = node.Advertise("actuators"); ignition::msgs::Actuators actuators_msg; @@ -242,6 +247,7 @@ int main(int /*argc*/, char **/*argv*/) imu_pub.Publish(imu_msg); laserscan_pub.Publish(laserscan_msg); magnetic_pub.Publish(magnetometer_msg); + navsat_pub.Publish(navsat_msg); actuators_pub.Publish(actuators_msg); map_pub.Publish(map_msg); odometry_pub.Publish(odometry_msg); diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index c2d35cf1..4acb75f9 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -220,6 +221,12 @@ int main(int argc, char ** argv) sensor_msgs::MagneticField magnetic_msg; ros_ign_bridge::testing::createTestMsg(magnetic_msg); + // sensor_msgs::NavSatFix. + ros::Publisher navsat_pub = + n.advertise("navsat", 1000); + sensor_msgs::NavSatFix navsat_msg; + ros_ign_bridge::testing::createTestMsg(navsat_msg); + // sensor_msgs::PointCloud2. ros::Publisher pointcloud2_pub = n.advertise("pointcloud2", 1000); @@ -275,6 +282,7 @@ int main(int argc, char ** argv) imu_pub.publish(imu_msg); laserscan_pub.publish(laserscan_msg); magnetic_pub.publish(magnetic_msg); + navsat_pub.publish(navsat_msg); joint_states_pub.publish(joint_states_msg); pointcloud2_pub.publish(pointcloud2_msg); battery_state_pub.publish(battery_state_msg); diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp index 35ac6381..eddfab50 100644 --- a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -335,6 +335,18 @@ TEST(IgnSubscriberTest, Magnetometer) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, NavSat) +{ + MyTestClass client("navsat"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// //TEST(IgnSubscriberTest, Actuators) //{ diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber.cpp index 6adf01d5..b403cbb7 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -391,6 +392,17 @@ TEST(ROSSubscriberTest, MagneticField) EXPECT_TRUE(client.callbackExecuted); } +TEST(ROSSubscriberTest, NavSatFix) +{ + MyTestClass client("navsat"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVarAndSpin( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// // TEST(ROSSubscriberTest, Actuators) // { diff --git a/ros_ign_bridge/test/test_utils.h b/ros_ign_bridge/test/test_utils.h index 8b324bf2..67e421a2 100644 --- a/ros_ign_bridge/test/test_utils.h +++ b/ros_ign_bridge/test/test_utils.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -832,6 +833,42 @@ namespace testing EXPECT_FLOAT_EQ(0, _msg.magnetic_field_covariance[i]); } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(sensor_msgs::NavSatFix &_msg) + { + std_msgs::Header header_msg; + createTestMsg(header_msg); + + _msg.header = header_msg; + _msg.status.status = sensor_msgs::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::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const sensor_msgs::NavSatFix &_msg) + { + sensor_msgs::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]); + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(sensor_msgs::PointCloud2 &_msg) @@ -1633,6 +1670,39 @@ namespace testing compareTestMsg(_msg.field_tesla()); } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + 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); + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const ignition::msgs::NavSat &_msg) + { + ignition::msgs::NavSat expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_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()); + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::Actuators &_msg) diff --git a/ros_ign_gazebo_demos/README.md b/ros_ign_gazebo_demos/README.md index a6503a57..f946750c 100644 --- a/ros_ign_gazebo_demos/README.md +++ b/ros_ign_gazebo_demos/README.md @@ -92,6 +92,14 @@ Publishes magnetic field readings. ![](images/magnetometer_demo.png) +## GNSS + +Publishes satellite navigation readings, only available in Ignition releases from Fortress on. + + roslaunch ros_ign_gazebo_demos navsat.launch + +![](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 b/ros_ign_gazebo_demos/launch/navsat.launch new file mode 100644 index 00000000..15420c1f --- /dev/null +++ b/ros_ign_gazebo_demos/launch/navsat.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + +