Use rosgraph_msgs/Clock for /clock topic. (#474)

* Use rosgraph_msgs/Clock for /clock topic.

* Update the test cases.

rclcpp/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
@@ -99,6 +100,7 @@ add_library(${PROJECT_NAME}
 ament_target_dependencies(${PROJECT_NAME}
   "rcl"
   "builtin_interfaces"
+  "rosgraph_msgs"
   "rosidl_typesupport_cpp"
   "rosidl_generator_cpp")
 
@@ -121,6 +123,7 @@ ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl)
 ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
 ament_export_dependencies(rosidl_typesupport_c)
 ament_export_dependencies(rosidl_generator_cpp)

rclcpp/include/rclcpp/time_source.hpp

@@ -21,6 +21,7 @@
 #include "rcl/time.h"
 
 #include "builtin_interfaces/msg/time.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
@@ -75,13 +76,13 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
 
   // The subscription for the clock callback
-  using MessageT = builtin_interfaces::msg::Time;
+  using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
   std::shared_ptr<SubscriptionT> clock_subscription_;
 
   // The clock callback itself
-  void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg);
+  void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
 
   // Parameter Client pointer
   std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
@@ -115,7 +116,7 @@ private:
   // This is needed when new clocks are added.
   bool ros_time_active_;
   // Last set message to be passed to newly registered clocks
-  builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
+  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
   // A lock to protect iterating the associated_clocks_ field.
   std::mutex clock_list_lock_;

rclcpp/package.xml

@@ -13,11 +13,13 @@
 
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
+  <build_export_depend>rosgraph_msgs</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>

rclcpp/src/rclcpp/time_source.cpp

@@ -118,7 +118,8 @@ void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
   associated_clocks_.push_back(clock);
   // Set the clock if there's already data for it
   if (last_msg_set_) {
-    set_clock(last_msg_set_, ros_time_active_, clock);
+    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
+    set_clock(time_msg, ros_time_active_, clock);
   }
 }
 
@@ -194,17 +195,18 @@ void TimeSource::set_clock(
   clock->invoke_postjump_callbacks(active_callbacks, jump);
 }
 
-void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg)
+void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
 {
   if (!this->ros_time_active_) {
     enable_ros_time();
   }
   // Cache the last message in case a new clock is attached.
   last_msg_set_ = msg;
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
 
   std::lock_guard<std::mutex> guard(clock_list_lock_);
   for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    set_clock(msg, true, *it);
+    set_clock(time_msg, true, *it);
   }
 }
 

rclcpp/test/test_time_source.cpp

@@ -92,18 +92,17 @@ TEST_F(TestTimeSource, clock) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
   rclcpp::WallRate loop_rate(50);
   for (int i = 0; i < 5; ++i) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->sec = i;
+    msg->clock.sec = i;
-    msg->nanosec = 1000;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -162,7 +161,7 @@ TEST_F(TestTimeSource, callbacks) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
 
   rclcpp::WallRate loop_rate(50);
@@ -170,11 +169,10 @@ TEST_F(TestTimeSource, callbacks) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->sec = i;
+    msg->clock.sec = i;
-    msg->nanosec = 1000;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -204,11 +202,10 @@ TEST_F(TestTimeSource, callbacks) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->sec = i;
+    msg->clock.sec = i;
-    msg->nanosec = 2000;
+    msg->clock.nanosec = 2000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -229,7 +226,7 @@ TEST_F(TestTimeSource, callbacks) {
 void trigger_clock_changes(
   rclcpp::Node::SharedPtr node)
 {
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
 
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -240,11 +237,10 @@ void trigger_clock_changes(
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->sec = i;
+    msg->clock.sec = i;
-    msg->nanosec = 1000;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     executor.spin_once(1000000ns);
     loop_rate.sleep();
   }