Destroy subscription when not needed, avoid re-creating it

dhood · dhood · commit 9ddb1ae7faf2 · 2018-09-21T12:51:56.000-07:00
diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp
@@ -80,13 +80,17 @@ class TimeSource
   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_;
 
diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp
@@ -37,6 +37,7 @@ TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
 : ros_time_active_(false)
 {
   this->attachNode(node);
+  clock_subscription_.reset();
 }
 
 TimeSource::TimeSource()
@@ -158,6 +159,11 @@ void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
 
 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;
@@ -180,6 +186,16 @@ void TimeSource::create_clock_sub()
     allocator);
 }
 
+void TimeSource::destroy_clock_sub()
+{
+  std::lock_guard<std::mutex> guard(clock_sub_lock_);
+  if (!clock_subscription_) {
+    // Subscription already destroyed/never created.
+    return;
+  }
+  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.
@@ -198,6 +214,7 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
     } else {
       parameter_state_ = SET_FALSE;
       disable_ros_time();
+      destroy_clock_sub();
     }
   }
   // Handle the case that use_sim_time was deleted.

Original file line number	Diff line number	Diff line change
`@@ -37,6 +37,7 @@ TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)`
`37`	`37`	`: ros_time_active_(false)`
`38`	`38`	`{`
`39`	`39`	`this->attachNode(node);`
	`40`	`+ clock_subscription_.reset();`
`40`	`41`	`}`
`41`	`42`
`42`	`43`	`TimeSource::TimeSource()`
`@@ -158,6 +159,11 @@ void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)`
`158`	`159`
`159`	`160`	`void TimeSource::create_clock_sub()`
`160`	`161`	`{`
	`162`	`+ std::lock_guard<std::mutex> guard(clock_sub_lock_);`
	`163`	`+ if (clock_subscription_) {`
	`164`	`+ // Subscription already created.`
	`165`	`+ return;`
	`166`	`+ }`
`161`	`167`	`const std::string topic_name = "/clock";`
`162`	`168`
`163`	`169`	`rclcpp::callback_group::CallbackGroup::SharedPtr group;`
`@@ -180,6 +186,16 @@ void TimeSource::create_clock_sub()`
`180`	`186`	`allocator);`
`181`	`187`	`}`
`182`	`188`
	`189`	`+void TimeSource::destroy_clock_sub()`
	`190`	`+{`
	`191`	`+ std::lock_guard<std::mutex> guard(clock_sub_lock_);`
	`192`	`+ if (!clock_subscription_) {`
	`193`	`+ // Subscription already destroyed/never created.`
	`194`	`+ return;`
	`195`	`+ }`
	`196`	`+ clock_subscription_.reset();`
	`197`	`+}`
	`198`	`+`
`183`	`199`	`void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)`
`184`	`200`	`{`
`185`	`201`	`// Filter for only 'use_sim_time' being added or changed.`
`@@ -198,6 +214,7 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S`
`198`	`214`	`} else {`
`199`	`215`	`parameter_state_ = SET_FALSE;`
`200`	`216`	`disable_ros_time();`
	`217`	`+ destroy_clock_sub();`
`201`	`218`	`}`
`202`	`219`	`}`
`203`	`220`	`// Handle the case that use_sim_time was deleted.`