diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 136652f..9b098a4 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -80,10 +80,17 @@ private: using Alloc = std::allocator; using SubscriptionT = rclcpp::Subscription; std::shared_ptr clock_subscription_; + std::mutex clock_sub_lock_; // The clock callback itself void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg); + // Create the subscription for the clock topic + void create_clock_sub(); + + // Destroy the subscription for the clock topic + void destroy_clock_sub(); + // Parameter Client pointer std::shared_ptr parameter_client_; diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index a18400a..830ed0e 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -34,7 +34,8 @@ namespace rclcpp { TimeSource::TimeSource(std::shared_ptr node) -: ros_time_active_(false) +: clock_subscription_(nullptr), + ros_time_active_(false) { this->attachNode(node); } @@ -65,28 +66,6 @@ void TimeSource::attachNode( node_services_ = node_services_interface; // TODO(tfoote): Update QOS - const std::string topic_name = "/clock"; - - rclcpp::callback_group::CallbackGroup::SharedPtr group; - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - auto msg_mem_strat = MessageMemoryStrategy::create_default(); - auto allocator = std::make_shared(); - - auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - - clock_subscription_ = rclcpp::create_subscription< - MessageT, decltype(cb), Alloc, MessageT, SubscriptionT - >( - node_topics_.get(), - topic_name, - std::move(cb), - rmw_qos_profile_default, - group, - false, - false, - msg_mem_strat, - allocator); - parameter_client_ = std::make_shared( node_base_, node_topics_, @@ -163,19 +142,56 @@ void TimeSource::set_clock( void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg) { - if (!this->ros_time_active_) { + if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { 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(time_msg, true, *it); + if (SET_TRUE == this->parameter_state_) { + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(time_msg, true, *it); + } } } +void TimeSource::create_clock_sub() +{ + std::lock_guard guard(clock_sub_lock_); + if (clock_subscription_) { + // Subscription already created. + return; + } + const std::string topic_name = "/clock"; + + rclcpp::callback_group::CallbackGroup::SharedPtr group; + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + auto msg_mem_strat = MessageMemoryStrategy::create_default(); + auto allocator = std::make_shared(); + auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); + + clock_subscription_ = rclcpp::create_subscription< + MessageT, decltype(cb), Alloc, MessageT, SubscriptionT + >( + node_topics_.get(), + topic_name, + std::move(cb), + rmw_qos_profile_default, + group, + false, + false, + msg_mem_strat, + allocator); +} + +void TimeSource::destroy_clock_sub() +{ + std::lock_guard guard(clock_sub_lock_); + clock_subscription_.reset(); +} + void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) { // Filter for only 'use_sim_time' being added or changed. @@ -190,9 +206,11 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S if (it.second->value.bool_value) { parameter_state_ = SET_TRUE; enable_ros_time(); + create_clock_sub(); } else { parameter_state_ = SET_FALSE; disable_ros_time(); + destroy_clock_sub(); } } // Handle the case that use_sim_time was deleted. diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index bc51820..dbb4fd7 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -73,6 +73,24 @@ void trigger_clock_changes( } } +void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value) +{ + auto parameters_client = std::make_shared(node); + + using namespace std::chrono_literals; + EXPECT_TRUE(parameters_client->wait_for_service(2s)); + auto set_parameters_results = parameters_client->set_parameters({ + rclcpp::Parameter("use_sim_time", value) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + // SyncParametersClient returns when parameters have been set on the node_parameters interface, + // but it doesn't mean the on_parameter_event subscription in TimeSource has been called. + // Spin some to handle that subscription. + rclcpp::spin_some(node); +} + TEST_F(TestTimeSource, detachUnattached) { rclcpp::TimeSource ts; @@ -117,14 +135,21 @@ TEST_F(TestTimeSource, clock) { trigger_clock_changes(node); - auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); - auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + // Even now that we've recieved a message, ROS time should still not be active since the + // parameter has not been explicitly set. + EXPECT_FALSE(ros_clock->ros_time_is_active()); - // Now that we've recieved a message it should be active with parameter unset + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); EXPECT_TRUE(ros_clock->ros_time_is_active()); + trigger_clock_changes(node); + auto t_out = ros_clock->now(); + // Time from clock should now reflect what was published on the /clock topic. + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); EXPECT_NE(0L, t_out.nanoseconds()); EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); @@ -179,18 +204,25 @@ TEST_F(TestTimeSource, callbacks) { auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); - EXPECT_EQ(1, cbo.last_precallback_id_); - EXPECT_EQ(1, cbo.last_postcallback_id_); + // Callbacks will not be triggered since ROS time is not active. + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); - // Now that we've recieved a message it should be active with parameter unset + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); EXPECT_TRUE(ros_clock->ros_time_is_active()); + trigger_clock_changes(node); + auto t_out = ros_clock->now(); EXPECT_NE(0L, t_out.nanoseconds()); EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + // Callbacks will now have been triggered since ROS time is active. + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); // Change callbacks rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( @@ -203,7 +235,6 @@ TEST_F(TestTimeSource, callbacks) { EXPECT_EQ(2, cbo.last_precallback_id_); EXPECT_EQ(2, cbo.last_postcallback_id_); - // Now that we've recieved a message it should be active with parameter unset EXPECT_TRUE(ros_clock->ros_time_is_active()); t_out = ros_clock->now(); @@ -258,24 +289,23 @@ TEST_F(TestTimeSource, callback_handler_erasure) { std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), jump_threshold); - + // Callbacks will not be triggered since ROS time is not active. EXPECT_EQ(0, cbo.last_precallback_id_); EXPECT_EQ(0, cbo.last_postcallback_id_); - - EXPECT_FALSE(ros_clock->ros_time_is_active()); + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + EXPECT_TRUE(ros_clock->ros_time_is_active()); trigger_clock_changes(node); auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + // Callbacks will now have been triggered since ROS time is active. EXPECT_EQ(1, cbo.last_precallback_id_); EXPECT_EQ(1, cbo.last_postcallback_id_); - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - auto t_out = ros_clock->now(); EXPECT_NE(0L, t_out.nanoseconds()); @@ -293,11 +323,9 @@ TEST_F(TestTimeSource, callback_handler_erasure) { trigger_clock_changes(node); - EXPECT_EQ(2, cbo.last_precallback_id_); EXPECT_EQ(2, cbo.last_postcallback_id_); - // Now that we've recieved a message it should be active with parameter unset EXPECT_TRUE(ros_clock->ros_time_is_active()); t_out = ros_clock->now(); @@ -316,48 +344,28 @@ TEST_F(TestTimeSource, parameter_activation) { ts.attachClock(ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - auto parameters_client = std::make_shared(node); - - using namespace std::chrono_literals; - EXPECT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters({ - rclcpp::Parameter("use_sim_time", true) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - // SyncParametersClient returns when parameters have been set on the node_parameters interface, - // but it doesn't mean the on_parameter_event subscription in TimeSource has been called. - // Spin some to handle that subscription. - rclcpp::spin_some(node); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); EXPECT_TRUE(ros_clock->ros_time_is_active()); - set_parameters_results = parameters_client->set_parameters({ - rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - rclcpp::spin_some(node); + set_use_sim_time_parameter( + node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET)); EXPECT_TRUE(ros_clock->ros_time_is_active()); - set_parameters_results = parameters_client->set_parameters({ - rclcpp::Parameter("use_sim_time", false) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - rclcpp::spin_some(node); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false)); EXPECT_FALSE(ros_clock->ros_time_is_active()); - set_parameters_results = parameters_client->set_parameters({ - rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - rclcpp::spin_some(node); + set_use_sim_time_parameter( + node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET)); EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time + // should not be affected by the presence of a clock publisher. + trigger_clock_changes(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false)); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + EXPECT_TRUE(ros_clock->ros_time_is_active()); } TEST_F(TestTimeSource, no_pre_jump_callback) { @@ -381,18 +389,7 @@ TEST_F(TestTimeSource, no_pre_jump_callback) { ts.attachClock(ros_clock); // Activate ROS time - auto parameters_client = std::make_shared(node); - ASSERT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters({ - rclcpp::Parameter("use_sim_time", true) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - // SyncParametersClient returns when parameters have been set on the node_parameters interface, - // but it doesn't mean the on_parameter_event subscription in TimeSource has been called. - // Spin some to handle that subscription. - rclcpp::spin_some(node); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); ASSERT_TRUE(ros_clock->ros_time_is_active()); EXPECT_EQ(0, cbo.last_precallback_id_);