Cleanup time source object lifetimes (#1867)

clalancette · web-flow · commit c54a6f1cd2b5 · 2022-01-20T16:30:55.000-05:00
* Small cleanups in TimeSource.

Simplify some code, and also make sure to throw an exception
when use_sim_time is not of type bool.  Also add a test for
the latter.

* Remove the sim_time_cb_handler.

It was originally used to make sure that someone didn't change
the 'use_sim_time' type from boolean to something else.  But
since the parameters interface now does that automatically for
us, we don't need the explicit check here.

I can think of one situation that this allows that wasn't allowed
before.  If the user defined 'use_sim_time' as a parameter override
when constructing a node, and the type is bool, and they also
defined the parameters as 'dynamic_typing', then this callback
will still have effect.  But presumably if the user went out of
their way to change the parameter to dynamic_typing, they are
trying to do something esoteric and so we should just let them.

* ClocksState does not need to be a pointer.

Instead, make it a regular member variable.  That lets us get
rid of all of the special handling for when it is a weak_ptr
or not.  It's lifetime is now just that of NodeState.

* Stop using NodeState as a weak_ptr in the captures.

This ended up causing the lifetime of the object to be weird.
Instead, just capture 'this' which is sufficient.

* Make sure destroy_clock_sub is first.

* Switch to using just a regular object.

Windows objects to trying to do "make_shared" in the RCLCPP
macro, so just switch to a normal object here.

Signed-off-by: Chris Lalancette &lt;clalancette@openrobotics.org&gt;
diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp
@@ -147,9 +147,6 @@ class TimeSource
   ~TimeSource();
 
 private:
-  class ClocksState;
-  std::shared_ptr<ClocksState> clocks_state_;
-
   class NodeState;
   std::shared_ptr<NodeState> node_state_;
 
diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp
@@ -33,7 +33,7 @@
 namespace rclcpp
 {
 
-class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
+class ClocksState final
 {
 public:
   ClocksState()
@@ -182,12 +182,11 @@ class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
   std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
 };
 
-class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
+class TimeSource::NodeState final
 {
 public:
-  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),
+  NodeState(const rclcpp::QoS & qos, bool use_clock_thread)
+  : use_clock_thread_(use_clock_thread),
     logger_(rclcpp::get_logger("rclcpp")),
     qos_(qos)
   {
@@ -250,94 +249,77 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
     if (!node_parameters_->has_parameter(use_sim_time_name)) {
       use_sim_time_param = node_parameters_->declare_parameter(
         use_sim_time_name,
-        rclcpp::ParameterValue(false),
-        rcl_interfaces::msg::ParameterDescriptor());
+        rclcpp::ParameterValue(false));
     } else {
       use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
     }
     if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
       if (use_sim_time_param.get<bool>()) {
         parameter_state_ = SET_TRUE;
-        // 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();
-        }
+        clocks_state_.enable_ros_time();
+        create_clock_sub();
       }
     } else {
       RCLCPP_ERROR(
         logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
         rclcpp::to_string(use_sim_time_param.get_type()).c_str());
+      throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'");
     }
-    sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
-      [use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = true;
-        for (const auto & parameter : parameters) {
-          if (
-            parameter.get_name() == use_sim_time_name &&
-            parameter.get_type() != rclcpp::PARAMETER_BOOL)
-          {
-            result.successful = false;
-            result.reason = "'" + use_sim_time_name + "' must be a bool";
-            break;
-          }
-        }
-        return result;
-      });
 
     // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
     parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
       node_topics_,
-      [state = std::weak_ptr<NodeState>(this->shared_from_this())](
-        std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
-        if (auto state_ptr = state.lock()) {
-          state_ptr->on_parameter_event(event);
+      [this](std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
+        if (node_base_ != nullptr) {
+          this->on_parameter_event(event);
         }
-        // Do nothing if the pointer could not be locked because it means the TimeSource is now
+        // Do nothing if node_base_ is nullptr because it means the TimeSource is now
         // without an attached node
       });
   }
 
   // Detach the attached node
   void detachNode()
   {
-    // 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() *must* be first here, to ensure that the executor
+    // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
+    clocks_state_.disable_ros_time();
     parameter_subscription_.reset();
     node_base_.reset();
     node_topics_.reset();
     node_graph_.reset();
     node_services_.reset();
     node_logging_.reset();
     node_clock_.reset();
-    if (sim_time_cb_handler_ && node_parameters_) {
-      node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
-    }
-    sim_time_cb_handler_.reset();
     node_parameters_.reset();
   }
 
+  void attachClock(std::shared_ptr<rclcpp::Clock> clock)
+  {
+    clocks_state_.attachClock(std::move(clock));
+  }
+
+  void detachClock(std::shared_ptr<rclcpp::Clock> clock)
+  {
+    clocks_state_.detachClock(std::move(clock));
+  }
+
 private:
-  std::weak_ptr<ClocksState> clocks_state_;
+  ClocksState clocks_state_;
 
   // Dedicated thread for clock subscription.
   bool use_clock_thread_;
   std::thread clock_executor_thread_;
 
   // Preserve the node reference
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
-  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
-  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
-  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr};
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr};
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr};
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_{nullptr};
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_{nullptr};
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr};
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_{nullptr};
 
   // Store (and update on node attach) logger for logging.
   Logger logger_;
@@ -346,9 +328,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   rclcpp::QoS qos_;
 
   // The subscription for the clock callback
-  using MessageT = rosgraph_msgs::msg::Clock;
-  using Alloc = std::allocator<void>;
-  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
+  using SubscriptionT = rclcpp::Subscription<rosgraph_msgs::msg::Clock>;
   std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
   std::mutex clock_sub_lock_;
   rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
@@ -358,21 +338,15 @@ 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)
   {
-    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();
+    if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) {
+      clocks_state_.enable_ros_time();
     }
     // Cache the last message in case a new clock is attached.
-    clocks_state_ptr->cache_last_msg(msg);
+    clocks_state_.cache_last_msg(msg);
     auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
 
     if (SET_TRUE == this->parameter_state_) {
-      clocks_state_ptr->set_all_clocks(time_msg, true);
+      clocks_state_.set_all_clocks(time_msg, true);
     }
   }
 
@@ -421,13 +395,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
       node_topics_,
       "/clock",
       qos_,
-      [state = std::weak_ptr<NodeState>(this->shared_from_this())](
-        std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
-        if (auto state_ptr = state.lock()) {
-          state_ptr->clock_cb(msg);
+      [this](std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
+        // We are using node_base_ as an indication if there is a node attached.
+        // Only call the clock_cb if that is the case.
+        if (node_base_ != nullptr) {
+          clock_cb(msg);
         }
-        // Do nothing if the pointer could not be locked because it means the TimeSource is now
-        // without an attached node
       },
       options
     );
@@ -447,19 +420,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
   }
 
   // Parameter Event subscription
-  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
-  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
+  using ParamSubscriptionT = rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>;
   std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
 
   // 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;
@@ -475,41 +441,35 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
       }
       if (it.second->value.bool_value) {
         parameter_state_ = SET_TRUE;
-        clocks_state_ptr->enable_ros_time();
+        clocks_state_.enable_ros_time();
         create_clock_sub();
       } else {
         parameter_state_ = SET_FALSE;
-        clocks_state_ptr->disable_ros_time();
         destroy_clock_sub();
+        clocks_state_.disable_ros_time();
       }
     }
     // Handle the case that use_sim_time was deleted.
     rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
       {rclcpp::ParameterEventsFilter::EventType::DELETED});
     for (auto & it : deleted.get_events()) {
       (void) it;  // if there is a match it's already matched, don't bother reading it.
-      // If the parameter is deleted mark it as unset but dont' change state.
+      // If the parameter is deleted mark it as unset but don't change state.
       parameter_state_ = UNSET;
     }
   }
 
   // An enum to hold the parameter state
   enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
   UseSimTimeParameterState parameter_state_;
-
-  // A handler for the use_sim_time parameter callback.
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
 };
 
 TimeSource::TimeSource(
   std::shared_ptr<rclcpp::Node> node,
   const rclcpp::QoS & qos,
   bool use_clock_thread)
-: constructed_use_clock_thread_(use_clock_thread),
-  constructed_qos_(qos)
+: TimeSource(qos, use_clock_thread)
 {
-  clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
   attachNode(node);
 }
 
@@ -519,8 +479,7 @@ TimeSource::TimeSource(
 : constructed_use_clock_thread_(use_clock_thread),
   constructed_qos_(qos)
 {
-  clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
+  node_state_ = std::make_shared<NodeState>(qos, use_clock_thread);
 }
 
 void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
@@ -559,19 +518,18 @@ void TimeSource::detachNode()
 {
   node_state_.reset();
   node_state_ = std::make_shared<NodeState>(
-    clocks_state_->weak_from_this(),
     constructed_qos_,
     constructed_use_clock_thread_);
 }
 
 void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
 {
-  clocks_state_->attachClock(std::move(clock));
+  node_state_->attachClock(std::move(clock));
 }
 
 void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 {
-  clocks_state_->detachClock(std::move(clock));
+  node_state_->detachClock(std::move(clock));
 }
 
 bool TimeSource::get_use_clock_thread()
diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp
@@ -27,6 +27,8 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/time_source.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 using namespace std::chrono_literals;
 
 class TestTimeSource : public ::testing::Test
@@ -246,11 +248,25 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
 }
 
 TEST_F(TestTimeSource, ROS_invalid_sim_time) {
-  rclcpp::TimeSource ts;
-  ts.attachNode(node);
+  rclcpp::TimeSource ts(node);
   EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful);
 }
 
+TEST(TimeSource, invalid_sim_time_parameter_override)
+{
+  rclcpp::init(0, nullptr);
+
+  rclcpp::NodeOptions options;
+  options.automatically_declare_parameters_from_overrides(true);
+  options.append_parameter_override("use_sim_time", "not boolean");
+
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::Node("my_node", options),
+    std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"));
+
+  rclcpp::shutdown();
+}
+
 TEST_F(TestTimeSource, clock) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
@@ -749,8 +765,7 @@ TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) {
 
   node->set_parameter({"use_sim_time", true});
   auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
-  rclcpp::TimeSource time_source;
-  time_source.attachNode(node);
+  rclcpp::TimeSource time_source(node);
   time_source.attachClock(clock);
 
   // Wait until time source has definitely received a first ROS time from the pub node
