Don't auto-activate ROS time if clock topic is being published (#559)

* Don't auto-activate ROS time if clock topic is being published

* Destroy subscription when not needed, avoid re-creating it

* Additional tests

* Always reset pointer

* Initialise sub in initialiser list

rclcpp/include/rclcpp/time_source.hpp

@@ -80,10 +80,17 @@ private:
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
   std::shared_ptr<SubscriptionT> 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<rclcpp::AsyncParametersClient> parameter_client_;
 

rclcpp/src/rclcpp/time_source.cpp

@@ -34,7 +34,8 @@ namespace rclcpp
 {
 
 TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> 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<MessageT, Alloc>::create_default();
-  auto allocator = std::make_shared<Alloc>();
-
-  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<rclcpp::AsyncParametersClient>(
     node_base_,
     node_topics_,
@@ -163,18 +142,55 @@ 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<builtin_interfaces::msg::Time>(msg->clock);
 
+  if (SET_TRUE == this->parameter_state_) {
     std::lock_guard<std::mutex> 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<std::mutex> 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<MessageT, Alloc>::create_default();
+  auto allocator = std::make_shared<Alloc>();
+  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<std::mutex> guard(clock_sub_lock_);
+  clock_subscription_.reset();
+}
 
 void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
 {
@@ -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.

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<rclcpp::SyncParametersClient>(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);
+  // Even now that we've recieved a message, ROS time should still not be active since the
-  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
+  // 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_);
+  // Callbacks will not be triggered since ROS time is not active.
-  EXPECT_EQ(1, cbo.last_postcallback_id_);
+  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_);
 
-
+  // Activate ROS time.
-  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());
 
   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<rclcpp::SyncParametersClient>(node);
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
-
-  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);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
+  set_use_sim_time_parameter(
-    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
+    node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
-    rclcpp::Parameter("use_sim_time", false)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
+  set_use_sim_time_parameter(
-    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
+    node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
   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<rclcpp::SyncParametersClient>(node);
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
-  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);
   ASSERT_TRUE(ros_clock->ros_time_is_active());
 
   EXPECT_EQ(0, cbo.last_precallback_id_);