diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index c6d0fd9f..0cbd1691 100644 --- a/ros_ign_bridge/README.md +++ b/ros_ign_bridge/README.md @@ -26,6 +26,7 @@ service calls. Its support is limited to only the following message types: | geometry_msgs/TransformStamped | ignition::msgs::Pose | | geometry_msgs/Twist | ignition::msgs::Twist | | mav_msgs/Actuators | ignition::msgs::Actuators | +| nav_msgs/OccupancyGrid | ignition::msgs::OccupancyGrid | | nav_msgs/Odometry | ignition::msgs::Odometry | | rosgraph_msgs/Clock | ignition::msgs::Clock | | sensor_msgs/BatteryState | ignition::msgs::BatteryState | diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert.hpp index 959e10e5..0a11b560 100644 --- a/ros_ign_bridge/include/ros_ign_bridge/convert.hpp +++ b/ros_ign_bridge/include/ros_ign_bridge/convert.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -302,6 +303,18 @@ convert_ign_to_ros( mav_msgs::Actuators & ros_msg); // nav_msgs +template<> +void +convert_ros_to_ign( + const nav_msgs::OccupancyGrid & ros_msg, + ignition::msgs::OccupancyGrid & ign_msg); + +template<> +void +convert_ign_to_ros( + const ignition::msgs::OccupancyGrid& ign_msg, + nav_msgs::OccupancyGrid & 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 c6eb16bf..bc38a3d4 100644 --- a/ros_ign_bridge/src/convert.cpp +++ b/ros_ign_bridge/src/convert.cpp @@ -534,6 +534,52 @@ convert_ign_to_ros( ros_msg.normalized.push_back(ign_msg.normalized(i)); } +template<> +void +convert_ros_to_ign( + const nav_msgs::OccupancyGrid & ros_msg, + ignition::msgs::OccupancyGrid & ign_msg) +{ + convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header())); + + ign_msg.mutable_info()->mutable_map_load_time()->set_sec( + ros_msg.info.map_load_time.sec); + ign_msg.mutable_info()->mutable_map_load_time()->set_nsec( + ros_msg.info.map_load_time.nsec); + + ign_msg.mutable_info()->set_resolution( + ros_msg.info.resolution); + ign_msg.mutable_info()->set_width( + ros_msg.info.width); + ign_msg.mutable_info()->set_height( + ros_msg.info.height); + + convert_ros_to_ign(ros_msg.info.origin, + (*ign_msg.mutable_info()->mutable_origin())); + + ign_msg.set_data(&ros_msg.data[0], ros_msg.data.size()); +} + +template<> +void +convert_ign_to_ros( + const ignition::msgs::OccupancyGrid & ign_msg, + nav_msgs::OccupancyGrid & ros_msg) +{ + convert_ign_to_ros(ign_msg.header(), ros_msg.header); + + ros_msg.info.map_load_time.sec = ign_msg.info().map_load_time().sec(); + ros_msg.info.map_load_time.nsec = ign_msg.info().map_load_time().nsec(); + ros_msg.info.resolution = ign_msg.info().resolution(); + ros_msg.info.width = ign_msg.info().width(); + ros_msg.info.height = ign_msg.info().height(); + + convert_ign_to_ros(ign_msg.info().origin(), ros_msg.info.origin); + + ros_msg.data.resize(ign_msg.data().size()); + memcpy(&ros_msg.data[0], ign_msg.data().c_str(), ign_msg.data().size()); +} + template<> void convert_ros_to_ign( diff --git a/ros_ign_bridge/src/factories.cpp b/ros_ign_bridge/src/factories.cpp index c79632d5..21edabfe 100644 --- a/ros_ign_bridge/src/factories.cpp +++ b/ros_ign_bridge/src/factories.cpp @@ -247,6 +247,17 @@ get_factory_impl( > >("mav_msgs/Actuators", ign_type_name); } + if ( + (ros_type_name == "nav_msgs/OccupancyGrid" || ros_type_name == "") && + ign_type_name == "ignition.msgs.OccupancyGrid") + { + return std::make_shared< + Factory< + nav_msgs::OccupancyGrid, + ignition::msgs::OccupancyGrid + > + >("nav_msgs/OccupancyGrid", ign_type_name); + } if ( (ros_type_name == "nav_msgs/Odometry" || ros_type_name == "") && ign_type_name == "ignition.msgs.Odometry") @@ -361,7 +372,6 @@ get_factory_impl( (ros_type_name == "visualization_msgs/Marker" || ros_type_name == "") && ign_type_name == "ignition.msgs.Marker") { - ROS_ERROR_STREAM("visualization_msgs/Marker"); return std::make_shared< Factory< visualization_msgs::Marker, @@ -373,7 +383,6 @@ get_factory_impl( (ros_type_name == "visualization_msgs/MarkerArray" || ros_type_name == "") && ign_type_name == "ignition.msgs.Marker_V") { - ROS_ERROR_STREAM("visualization_msgs/MarkerArray"); return std::make_shared< Factory< visualization_msgs::MarkerArray, @@ -883,6 +892,30 @@ Factory< } // nav_msgs +template<> +void +Factory< + nav_msgs::OccupancyGrid, + ignition::msgs::OccupancyGrid +>::convert_ros_to_ign( + const nav_msgs::OccupancyGrid & ros_msg, + ignition::msgs::OccupancyGrid & ign_msg) +{ + ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg); +} + +template<> +void +Factory< + nav_msgs::OccupancyGrid, + ignition::msgs::OccupancyGrid +>::convert_ign_to_ros( + const ignition::msgs::OccupancyGrid & ign_msg, + nav_msgs::OccupancyGrid & 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 7202d010..d3cc348e 100644 --- a/ros_ign_bridge/src/factories.hpp +++ b/ros_ign_bridge/src/factories.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -430,6 +431,24 @@ Factory< mav_msgs::Actuators & ros_msg); // nav_msgs +template<> +void +Factory< + nav_msgs::OccupancyGrid, + ignition::msgs::OccupancyGrid +>::convert_ros_to_ign( + const nav_msgs::OccupancyGrid & ros_msg, + ignition::msgs::OccupancyGrid & ign_msg); + +template<> +void +Factory< + nav_msgs::OccupancyGrid, + ignition::msgs::OccupancyGrid +>::convert_ign_to_ros( + const ignition::msgs::OccupancyGrid & ign_msg, + nav_msgs::OccupancyGrid & 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 3192ab7c..961399d1 100644 --- a/ros_ign_bridge/test/launch/test_ign_subscriber.launch +++ b/ros_ign_bridge/test/launch/test_ign_subscriber.launch @@ -29,6 +29,7 @@ /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer /actuators@mav_msgs/Actuators@ignition.msgs.Actuators + /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid /odometry@nav_msgs/Odometry@ignition.msgs.Odometry /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked /joint_states@sensor_msgs/JointState@ignition.msgs.Model diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch b/ros_ign_bridge/test/launch/test_ros_subscriber.launch index 023db7ac..eaefb042 100644 --- a/ros_ign_bridge/test/launch/test_ros_subscriber.launch +++ b/ros_ign_bridge/test/launch/test_ros_subscriber.launch @@ -29,6 +29,7 @@ /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer /actuators@mav_msgs/Actuators@ignition.msgs.Actuators + /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid /odometry@nav_msgs/Odometry@ignition.msgs.Odometry /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked /joint_states@sensor_msgs/JointState@ignition.msgs.Model diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index b72cd4ac..24205a33 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -176,6 +176,11 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Actuators actuators_msg; ros_ign_bridge::testing::createTestMsg(actuators_msg); + // ignition::msgs::OccupancyGrid + auto map_pub = node.Advertise("map"); + ignition::msgs::OccupancyGrid map_msg; + ros_ign_bridge::testing::createTestMsg(map_msg); + // ignition::msgs::Odometry. auto odometry_pub = node.Advertise("odometry"); ignition::msgs::Odometry odometry_msg; @@ -238,6 +243,7 @@ int main(int /*argc*/, char **/*argv*/) laserscan_pub.Publish(laserscan_msg); magnetic_pub.Publish(magnetometer_msg); actuators_pub.Publish(actuators_msg); + map_pub.Publish(map_msg); odometry_pub.Publish(odometry_msg); joint_states_pub.Publish(joint_states_msg); twist_pub.Publish(twist_msg); diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index 818649f9..c688d177 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -165,6 +166,12 @@ int main(int argc, char ** argv) mav_msgs::Actuators actuators_msg; ros_ign_bridge::testing::createTestMsg(actuators_msg); + // nav_msgs::OccupancyGrid. + ros::Publisher map_pub = + n.advertise("map", 1000); + nav_msgs::OccupancyGrid map_msg; + ros_ign_bridge::testing::createTestMsg(map_msg); + // nav_msgs::Odometry. ros::Publisher odometry_pub = n.advertise("odometry", 1000); @@ -260,6 +267,7 @@ int main(int argc, char ** argv) tf2_message_pub.publish(tf2_msg); twist_pub.publish(twist_msg); actuators_pub.publish(actuators_msg); + map_pub.publish(map_msg); odometry_pub.publish(odometry_msg); image_pub.publish(image_msg); camera_info_pub.publish(camera_info_msg); diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp index deb010ad..5ff09e06 100644 --- a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp @@ -347,6 +347,18 @@ TEST(IgnSubscriberTest, Actuators) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, OccupancyGrid) +{ + MyTestClass client("map"); + + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(IgnSubscriberTest, Odometry) { diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber.cpp index 40d74222..19517848 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -395,6 +396,18 @@ TEST(ROSSubscriberTest, Actuators) { MyTestClass client("actuators"); + using namespace std::chrono_literals; + ros_ign_bridge::testing::waitUntilBoolVarAndSpin( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + +///////////////////////////////////////////////// +TEST(ROSSubscriberTest, OccupancyGrid) +{ + MyTestClass client("map"); + using namespace std::chrono_literals; ros_ign_bridge::testing::waitUntilBoolVarAndSpin( client.callbackExecuted, 10ms, 200); diff --git a/ros_ign_bridge/test/test_utils.h b/ros_ign_bridge/test/test_utils.h index a186aa3a..8fba9b05 100644 --- a/ros_ign_bridge/test/test_utils.h +++ b/ros_ign_bridge/test/test_utils.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -492,6 +493,43 @@ namespace testing } } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(nav_msgs::OccupancyGrid &_msg) + { + createTestMsg(_msg.header); + + _msg.info.map_load_time.sec = 100; + _msg.info.map_load_time.nsec = 200; + _msg.info.resolution = 0.05; + _msg.info.width = 10; + _msg.info.height = 20; + createTestMsg(_msg.info.origin); + _msg.data.resize(_msg.info.width * _msg.info.height, 1); + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const nav_msgs::OccupancyGrid &_msg) + { + nav_msgs::OccupancyGrid expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg.header); + EXPECT_EQ(expected_msg.info.map_load_time.sec, + _msg.info.map_load_time.sec); + EXPECT_EQ(expected_msg.info.map_load_time.nsec, + _msg.info.map_load_time.nsec); + EXPECT_FLOAT_EQ(expected_msg.info.resolution, _msg.info.resolution); + EXPECT_EQ(expected_msg.info.width, _msg.info.width); + EXPECT_EQ(expected_msg.info.height, _msg.info.height); + + compareTestMsg(_msg.info.origin); + + EXPECT_EQ(expected_msg.data.size(), _msg.data.size()); + EXPECT_EQ(expected_msg.data[0], _msg.data[0]); + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(nav_msgs::Odometry &_msg) @@ -899,7 +937,6 @@ namespace testing /// \param[out] _msg The message populated. void createTestMsg(visualization_msgs::Marker &_msg) { - ROS_ERROR_STREAM("createTestMsg: Marker"); createTestMsg(_msg.header); _msg.ns = "foo"; @@ -950,7 +987,6 @@ namespace testing /// \param[out] _msg The message populated. void createTestMsg(visualization_msgs::MarkerArray &_msg) { - ROS_ERROR_STREAM("createTestMsg: MarkerArray"); _msg.markers.clear(); visualization_msgs::Marker marker; createTestMsg(marker); @@ -1631,6 +1667,47 @@ namespace testing } } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(ignition::msgs::OccupancyGrid &_msg) + { + ignition::msgs::Header header_msg; + ignition::msgs::Pose pose_msg; + + createTestMsg(header_msg); + createTestMsg(pose_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + + _msg.mutable_info()->mutable_map_load_time()->set_sec(100); + _msg.mutable_info()->mutable_map_load_time()->set_nsec(200); + _msg.mutable_info()->set_resolution(0.05); + _msg.mutable_info()->set_width(10); + _msg.mutable_info()->set_height(20); + + _msg.mutable_info()->mutable_origin()->CopyFrom(pose_msg); + + std::vector data(_msg.info().height() * _msg.info().width(), 1); + _msg.set_data(&data[0], data.size()); + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const ignition::msgs::OccupancyGrid &_msg) + { + compareTestMsg(_msg.header()); + + EXPECT_EQ(100, _msg.info().map_load_time().sec()); + EXPECT_EQ(200, _msg.info().map_load_time().nsec()); + EXPECT_FLOAT_EQ(0.05, _msg.info().resolution()); + EXPECT_EQ(10u, _msg.info().width()); + EXPECT_EQ(20u, _msg.info().height()); + compareTestMsg(_msg.info().origin()); + + EXPECT_EQ(20u * 10u, _msg.data().size()); + EXPECT_EQ('\1', _msg.data()[0]); + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::Odometry &_msg)