From d6057270f2c384361ac67b4804d569506b2ce598 Mon Sep 17 00:00:00 2001 From: Ernesto Corbellini Date: Wed, 30 May 2018 20:32:08 -0300 Subject: [PATCH] Use rosgraph_msgs/Clock for /clock topic. (#474) * Use rosgraph_msgs/Clock for /clock topic. * Update the test cases. --- rclcpp/CMakeLists.txt | 3 +++ rclcpp/include/rclcpp/time_source.hpp | 7 +++--- rclcpp/package.xml | 2 ++ rclcpp/src/rclcpp/time_source.cpp | 8 ++++--- rclcpp/test/test_time_source.cpp | 34 ++++++++++++--------------- 5 files changed, 29 insertions(+), 25 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 523ca3f..f8caf59 100644 --- a/rclcpp/CMakeLists.txt +++ b/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) diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 0d6f8da..136652f 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/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; using SubscriptionT = rclcpp::Subscription; std::shared_ptr 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 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_; diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 4b55dcc..686caa8 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -13,11 +13,13 @@ builtin_interfaces rcl_interfaces + rosgraph_msgs rosidl_generator_cpp rosidl_typesupport_c rosidl_typesupport_cpp builtin_interfaces rcl_interfaces + rosgraph_msgs rosidl_generator_cpp rosidl_typesupport_c rosidl_typesupport_cpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 54c72db..d5ebd1b 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -118,7 +118,8 @@ void TimeSource::attachClock(std::shared_ptr 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(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(msg->clock); std::lock_guard 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); } } diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index be84780..8952817 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/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("clock", + auto clock_pub = node->create_publisher("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(); - msg->sec = i; - msg->nanosec = 1000; + auto msg = std::make_shared(); + msg->clock.sec = i; + 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("clock", + auto clock_pub = node->create_publisher("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(); - msg->sec = i; - msg->nanosec = 1000; + auto msg = std::make_shared(); + msg->clock.sec = i; + 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(); - msg->sec = i; - msg->nanosec = 2000; + auto msg = std::make_shared(); + msg->clock.sec = i; + 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("clock", + auto clock_pub = node->create_publisher("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(); - msg->sec = i; - msg->nanosec = 1000; + auto msg = std::make_shared(); + msg->clock.sec = i; + 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(); }