Add clock message support (#2)

Shokman · caguero · commit 3debaf550aae · 2019-03-20T14:34:25.000-07:00
* Add clock message support

* Add rosgraph_msgs dependency

* Fix tests from merge

* Add  methods to headers convert and builtin
diff --git 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
@@ -23,6 +23,7 @@
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Vector3.h>
+#include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
@@ -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
@@ -21,6 +21,7 @@
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3.h>
+#include <rosgraph_msgs/Clock.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <sensor_msgs/FluidPressure.h>
@@ -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
@@ -8,6 +8,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>rosgraph_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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<ignition::msgs::Clock>("clock");
+  ignition::msgs::Clock clock_msg;
+  ros1_ign_bridge::testing::createTestMsg(clock_msg);
+
   // ignition::msgs::Point.
   auto point_pub = node.Advertise<ignition::msgs::Vector3d>("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
@@ -19,6 +19,7 @@
 #include <std_msgs/String.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
+#include <rosgraph_msgs/Clock.h>
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Transform.h>
@@ -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<rosgraph_msgs::Clock>("clock", 1000);
+  rosgraph_msgs::Clock clock_msg;
+  ros1_ign_bridge::testing::createTestMsg(clock_msg);
+
   // geometry_msgs::Point.
   ros::Publisher point_pub =
     n.advertise<geometry_msgs::Point>("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
@@ -95,6 +95,18 @@ TEST(IgnSubscriberTest, Vector3)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, Clock)
+{
+  MyTestClass<ignition::msgs::Clock> 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
@@ -26,6 +26,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
+#include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
@@ -109,6 +110,18 @@ TEST(ROS1SubscriberTest, Vector3)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(ROS1SubscriberTest, Clock)
+{
+  MyTestClass<rosgraph_msgs::Clock> 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
@@ -29,6 +29,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
+#include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
@@ -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)
