diff --git a/README.md b/README.md index b1b1f3dd..4bbe1367 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,7 @@ service calls.Its support is limited to only the following message types: | geometry_msgs/PoseStamped | ignition::msgs::Pose | | geometry_msgs/Transform | ignition::msgs::Pose | | geometry_msgs/TransformStamped | ignition::msgs::Pose | +| rosgraph_msgs/Clock | ignition::msgs::Clock | | sensor_msgs/Imu | ignition::msgs::IMU | | sensor_msgs/Image | ignition::msgs::Image | | sensor_msgs/LaserScan | ignition::msgs::LaserScan | diff --git a/include/ros1_ign_bridge/builtin_interfaces_factories.hpp b/include/ros1_ign_bridge/builtin_interfaces_factories.hpp index 8b2c1283..2378c1a9 100644 --- a/include/ros1_ign_bridge/builtin_interfaces_factories.hpp +++ b/include/ros1_ign_bridge/builtin_interfaces_factories.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -91,6 +92,25 @@ Factory< const ignition::msgs::StringMsg & ign_msg, std_msgs::String & ros1_msg); +// rosgraph_msgs +template<> +void +Factory< + rosgraph_msgs::Clock, + ignition::msgs::Clock +>::convert_1_to_ign( + const rosgraph_msgs::Clock & ros1_msg, + ignition::msgs::Clock & ign_msg); + +template<> +void +Factory< + rosgraph_msgs::Clock, + ignition::msgs::Clock +>::convert_ign_to_1( + const ignition::msgs::Clock & ign_msg, + rosgraph_msgs::Clock & ros1_msg); + // geometry_msgs template<> void diff --git a/include/ros1_ign_bridge/convert_builtin_interfaces.hpp b/include/ros1_ign_bridge/convert_builtin_interfaces.hpp index bdf1cc9a..1aaacf5c 100644 --- a/include/ros1_ign_bridge/convert_builtin_interfaces.hpp +++ b/include/ros1_ign_bridge/convert_builtin_interfaces.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,19 @@ convert_ign_to_1( const ignition::msgs::StringMsg & ign_msg, std_msgs::String & ros1_msg); +// rosgraph_msgs +template<> +void +convert_ign_to_1( + const ignition::msgs::Clock & ign_msg, + rosgraph_msgs::Clock & ros1_msg); + +template<> +void +convert_1_to_ign( + const rosgraph_msgs::Clock & ros1_msg, + ignition::msgs::Clock & ign_msg); + // geometry_msgs template<> void diff --git a/package.xml b/package.xml index 50dd8efa..b5753e3a 100644 --- a/package.xml +++ b/package.xml @@ -8,6 +8,7 @@ catkin geometry_msgs + rosgraph_msgs roscpp sensor_msgs std_msgs diff --git a/src/builtin_interfaces_factories.cpp b/src/builtin_interfaces_factories.cpp index e912695b..2840b20a 100644 --- a/src/builtin_interfaces_factories.cpp +++ b/src/builtin_interfaces_factories.cpp @@ -61,6 +61,17 @@ get_factory_builtin_interfaces( > >("geometry_msgs/Quaternion", ign_type_name); } + if ( + (ros1_type_name == "rosgraph_msgs/Clock" || ros1_type_name == "") && + ign_type_name == "ignition.msgs.Clock") + { + return std::make_shared< + Factory< + rosgraph_msgs::Clock, + ignition::msgs::Clock + > + >("rosgraph_msgs/Clock", ign_type_name); + } if ( (ros1_type_name == "geometry_msgs/Vector3" || ros1_type_name == "") && ign_type_name == "ignition.msgs.Vector3d") @@ -259,6 +270,31 @@ Factory< ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg); } +// rosgraph_msgs +template<> +void +Factory< + rosgraph_msgs::Clock, + ignition::msgs::Clock +>::convert_1_to_ign( + const rosgraph_msgs::Clock & ros1_msg, + ignition::msgs::Clock & ign_msg) +{ + ros1_ign_bridge::convert_1_to_ign(ros1_msg, ign_msg); +} + +template<> +void +Factory< + rosgraph_msgs::Clock, + ignition::msgs::Clock +>::convert_ign_to_1( + const ignition::msgs::Clock & ign_msg, + rosgraph_msgs::Clock & ros1_msg) +{ + ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg); +} + // geometry_msgs template<> void diff --git a/src/convert_builtin_interfaces.cpp b/src/convert_builtin_interfaces.cpp index ffee6f3f..956341a6 100644 --- a/src/convert_builtin_interfaces.cpp +++ b/src/convert_builtin_interfaces.cpp @@ -85,6 +85,25 @@ convert_ign_to_1( ros1_msg.data = ign_msg.data(); } +template<> +void +convert_ign_to_1( + const ignition::msgs::Clock & ign_msg, + rosgraph_msgs::Clock & ros1_msg) +{ + ros1_msg.clock = ros::Time(ign_msg.sim().sec(), ign_msg.sim().nsec()); +} + +template<> +void +convert_1_to_ign( + const rosgraph_msgs::Clock & ros1_msg, + ignition::msgs::Clock & ign_msg) +{ + ign_msg.mutable_sim()->set_sec(ros1_msg.clock.sec); + ign_msg.mutable_sim()->set_nsec(ros1_msg.clock.nsec); +} + template<> void convert_1_to_ign( diff --git a/test/launch/test_ign_subscriber.launch b/test/launch/test_ign_subscriber.launch index 5c0fa71c..0b4c67f0 100644 --- a/test/launch/test_ign_subscriber.launch +++ b/test/launch/test_ign_subscriber.launch @@ -7,6 +7,7 @@ /string@std_msgs/String@ignition.msgs.StringMsg /quaternion@geometry_msgs/Quaternion@ignition.msgs.Quaternion /vector3@geometry_msgs/Vector3@ignition.msgs.Vector3d + /clock@rosgraph_msgs/Clock@ignition.msgs.Clock /point@geometry_msgs/Point@ignition.msgs.Vector3d /pose@geometry_msgs/Pose@ignition.msgs.Pose /pose_stamped@geometry_msgs/PoseStamped@ignition.msgs.Pose diff --git a/test/launch/test_ros1_subscriber.launch b/test/launch/test_ros1_subscriber.launch index 081037c2..8f0d1275 100644 --- a/test/launch/test_ros1_subscriber.launch +++ b/test/launch/test_ros1_subscriber.launch @@ -7,6 +7,7 @@ /string@std_msgs/String@ignition.msgs.StringMsg /quaternion@geometry_msgs/Quaternion@ignition.msgs.Quaternion /vector3@geometry_msgs/Vector3@ignition.msgs.Vector3d + /clock@rosgraph_msgs/Clock@ignition.msgs.Clock /point@geometry_msgs/Point@ignition.msgs.Vector3d /pose@geometry_msgs/Pose@ignition.msgs.Pose /pose_stamped@geometry_msgs/PoseStamped@ignition.msgs.Pose diff --git a/test/publishers/ign_publisher.cpp b/test/publishers/ign_publisher.cpp index 5f13dff8..20f34c05 100644 --- a/test/publishers/ign_publisher.cpp +++ b/test/publishers/ign_publisher.cpp @@ -69,6 +69,11 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::Vector3d vector3_msg; ros1_ign_bridge::testing::createTestMsg(vector3_msg); + // ignition::msgs::Clock. + auto clock_pub = node.Advertise("clock"); + ignition::msgs::Clock clock_msg; + ros1_ign_bridge::testing::createTestMsg(clock_msg); + // ignition::msgs::Point. auto point_pub = node.Advertise("point"); ignition::msgs::Vector3d point_msg; @@ -123,6 +128,7 @@ int main(int /*argc*/, char **/*argv*/) string_pub.Publish(string_msg); quaternion_pub.Publish(quaternion_msg); vector3_pub.Publish(vector3_msg); + clock_pub.Publish(clock_msg); point_pub.Publish(point_msg); pose_pub.Publish(pose_msg); pose_stamped_pub.Publish(pose_stamped_msg); diff --git a/test/publishers/ros1_publisher.cpp b/test/publishers/ros1_publisher.cpp index 2cda31cc..84bc3bcc 100644 --- a/test/publishers/ros1_publisher.cpp +++ b/test/publishers/ros1_publisher.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,12 @@ int main(int argc, char ** argv) geometry_msgs::Vector3 vector3_msg; ros1_ign_bridge::testing::createTestMsg(vector3_msg); + // sensor_msgs::Clock. + ros::Publisher clock_pub = + n.advertise("clock", 1000); + rosgraph_msgs::Clock clock_msg; + ros1_ign_bridge::testing::createTestMsg(clock_msg); + // geometry_msgs::Point. ros::Publisher point_pub = n.advertise("point", 1000); @@ -119,6 +126,7 @@ int main(int argc, char ** argv) string_pub.publish(string_msg); quaternion_pub.publish(quaternion_msg); vector3_pub.publish(vector3_msg); + clock_pub.publish(clock_msg); point_pub.publish(point_msg); pose_pub.publish(pose_msg); pose_stamped_pub.publish(pose_stamped_msg); diff --git a/test/subscribers/ign_subscriber.cpp b/test/subscribers/ign_subscriber.cpp index a635fc95..036cd7d4 100644 --- a/test/subscribers/ign_subscriber.cpp +++ b/test/subscribers/ign_subscriber.cpp @@ -95,6 +95,18 @@ TEST(IgnSubscriberTest, Vector3) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(IgnSubscriberTest, Clock) +{ + MyTestClass client("clock"); + + using namespace std::chrono_literals; + ros1_ign_bridge::testing::waitUntilBoolVar( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(IgnSubscriberTest, Point) { diff --git a/test/subscribers/ros1_subscriber.cpp b/test/subscribers/ros1_subscriber.cpp index fb4d7244..9341f47b 100644 --- a/test/subscribers/ros1_subscriber.cpp +++ b/test/subscribers/ros1_subscriber.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -109,6 +110,18 @@ TEST(ROS1SubscriberTest, Vector3) EXPECT_TRUE(client.callbackExecuted); } +///////////////////////////////////////////////// +TEST(ROS1SubscriberTest, Clock) +{ + MyTestClass client("clock"); + + using namespace std::chrono_literals; + ros1_ign_bridge::testing::waitUntilBoolVarAndSpin( + client.callbackExecuted, 10ms, 200); + + EXPECT_TRUE(client.callbackExecuted); +} + ///////////////////////////////////////////////// TEST(ROS1SubscriberTest, Point) { diff --git a/test/test_utils.h b/test/test_utils.h index e541bee6..bd2b3688 100644 --- a/test/test_utils.h +++ b/test/test_utils.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -177,6 +178,14 @@ namespace testing EXPECT_EQ(3, _msg.z); } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(rosgraph_msgs::Clock &_msg) + { + _msg.clock.sec = 1; + _msg.clock.nsec = 2; + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::Point &_msg) @@ -186,6 +195,17 @@ namespace testing _msg.z = 3; } + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const rosgraph_msgs::Clock &_msg) + { + rosgraph_msgs::Clock expected_msg; + createTestMsg(expected_msg); + + EXPECT_EQ(expected_msg.clock.sec, _msg.clock.sec); + EXPECT_EQ(expected_msg.clock.nsec, _msg.clock.nsec); + } + /// \brief Compare a message with the populated for testing. /// \param[in] _msg The message to compare. void compareTestMsg(const geometry_msgs::Point &_msg) @@ -461,6 +481,25 @@ namespace testing EXPECT_EQ(expected_msg.data(1).value(0), _msg.data(1).value(0)); } + /// \brief Create a message used for testing. + /// \param[out] _msg The message populated. + void createTestMsg(ignition::msgs::Clock &_msg) + { + _msg.mutable_sim()->set_sec(1); + _msg.mutable_sim()->set_nsec(2); + } + + /// \brief Compare a message with the populated for testing. + /// \param[in] _msg The message to compare. + void compareTestMsg(const ignition::msgs::Clock &_msg) + { + ignition::msgs::Clock expected_msg; + createTestMsg(expected_msg); + + EXPECT_EQ(expected_msg.sim().sec(), _msg.sim().sec()); + EXPECT_EQ(expected_msg.sim().nsec(), _msg.sim().nsec()); + } + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ignition::msgs::StringMsg &_msg)