Use a weak_ptr to avoid a potential dangling reference

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>
ros2 · Sep 27, 2021 · 74c28d0 · 74c28d0
1 parent 8ca0100
commit 74c28d0
Showing 1 changed file with 35 additions and 15 deletions.
diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp
@@ -185,8 +185,8 @@ class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
 class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
 {
 public:
-  NodeState(ClocksState & clocks_state, const rclcpp::QoS & qos, bool use_clock_thread)
-  : clocks_state_(clocks_state),
+  NodeState(std::weak_ptr<ClocksState> clocks_state, const rclcpp::QoS & qos, bool use_clock_thread)
+  : clocks_state_(std::move(clocks_state)),
     use_clock_thread_(use_clock_thread),
     logger_(rclcpp::get_logger("rclcpp")),
     qos_(qos)
@@ -258,8 +258,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
     if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
       if (use_sim_time_param.get<bool>()) {
         parameter_state_ = SET_TRUE;
-        clocks_state_.enable_ros_time();
-        create_clock_sub();
+        // There should be no way to call this attachNode when the clocks_state_ pointer is not
+        // valid because it means the TimeSource is being destroyed
+        if (auto clocks_state_ptr = clocks_state_.lock()) {
+          clocks_state_ptr->enable_ros_time();
+          create_clock_sub();
+        }
       }
     } else {
       RCLCPP_ERROR(
@@ -299,7 +303,11 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   // Detach the attached node
   void detachNode()
   {
-    clocks_state_.disable_ros_time();
+    // There should be no way to call detachNode when the clocks_state_ pointer is not valid
+    // because it means the TimeSource is being destroyed
+    if (auto clocks_state_ptr = clocks_state_.lock()) {
+      clocks_state_ptr->disable_ros_time();
+    }
     destroy_clock_sub();
     parameter_subscription_.reset();
     node_base_.reset();
@@ -316,7 +324,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   }
 
 private:
-  ClocksState & clocks_state_;
+  std::weak_ptr<ClocksState> clocks_state_;
 
   // Dedicated thread for clock subscription.
   bool use_clock_thread_;
@@ -350,15 +358,21 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   // The clock callback itself
   void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
   {
-    if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) {
-      clocks_state_.enable_ros_time();
+    auto clocks_state_ptr = clocks_state_.lock();
+    if (!clocks_state_ptr) {
+      // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
+      // destroyed, so do nothing
+      return;
+    }
+    if (!clocks_state_ptr->is_ros_time_active() && SET_TRUE == this->parameter_state_) {
+      clocks_state_ptr->enable_ros_time();
     }
     // Cache the last message in case a new clock is attached.
-    clocks_state_.cache_last_msg(msg);
+    clocks_state_ptr->cache_last_msg(msg);
     auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
 
     if (SET_TRUE == this->parameter_state_) {
-      clocks_state_.set_all_clocks(time_msg, true);
+      clocks_state_ptr->set_all_clocks(time_msg, true);
     }
   }
 
@@ -440,6 +454,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   // Callback for parameter updates
   void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
   {
+    auto clocks_state_ptr = clocks_state_.lock();
+    if (!clocks_state_ptr) {
+      // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
+      // destroyed, so do nothing
+      return;
+    }
     // Filter out events on 'use_sim_time' parameter instances in other nodes.
     if (event->node != node_base_->get_fully_qualified_name()) {
       return;
@@ -455,11 +475,11 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
       }
       if (it.second->value.bool_value) {
         parameter_state_ = SET_TRUE;
-        clocks_state_.enable_ros_time();
+        clocks_state_ptr->enable_ros_time();
         create_clock_sub();
       } else {
         parameter_state_ = SET_FALSE;
-        clocks_state_.disable_ros_time();
+        clocks_state_ptr->disable_ros_time();
         destroy_clock_sub();
       }
     }
@@ -489,7 +509,7 @@ TimeSource::TimeSource(
   constructed_qos_(qos)
 {
   clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(*clocks_state_, qos, use_clock_thread);
+  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
   attachNode(node);
 }
 
@@ -500,7 +520,7 @@ TimeSource::TimeSource(
   constructed_qos_(qos)
 {
   clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(*clocks_state_, qos, use_clock_thread);
+  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
 }
 
 void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
@@ -539,7 +559,7 @@ void TimeSource::detachNode()
 {
   node_state_.reset();
   node_state_ = std::make_shared<NodeState>(
-    *clocks_state_,
+    clocks_state_->weak_from_this(),
     constructed_qos_,
     constructed_use_clock_thread_);
 }