Add callback handler for use_sim_time parameter #802

rclcpp/include/rclcpp/time_source.hpp

@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 
 
 namespace rclcpp
@@ -133,6 +134,8 @@ class TimeSource
   std::mutex clock_list_lock_;
   // A vector to store references to associated clocks.
   std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+  // A handler for the use_sim_time parameter callback.
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr;
 };
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/time_source.cpp

@@ -82,7 +82,7 @@ void TimeSource::attachNode(
   // Though this defaults to false, it can be overridden by initial parameter values for the node,
   // which may be given by the user at the node's construction or even by command-line arguments.
   rclcpp::ParameterValue use_sim_time_param;
-  const char * use_sim_time_name = "use_sim_time";
+  const std::string use_sim_time_name = "use_sim_time";
   if (!node_parameters_->has_parameter(use_sim_time_name)) {
     use_sim_time_param = node_parameters_->declare_parameter(
       use_sim_time_name,
@@ -98,12 +98,26 @@ void TimeSource::attachNode(
       create_clock_sub();
     }
   } else {
-    // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch,
-    //   before the use_sim_time parameter can ever be set to an invalid value
     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());
   }
+  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(
@@ -122,6 +136,10 @@ void TimeSource::detachNode()
   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();
   disable_ros_time();
 }
@@ -231,10 +249,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
     {rclcpp::ParameterEventsFilter::EventType::NEW,
       rclcpp::ParameterEventsFilter::EventType::CHANGED});
   for (auto & it : filter.get_events()) {
-    if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
-      RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
-      continue;
-    }
     if (it.second->value.bool_value) {
       parameter_state_ = SET_TRUE;
       enable_ros_time();

rclcpp/test/test_time_source.cpp

@@ -242,6 +242,12 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
   EXPECT_TRUE(ros_clock2->ros_time_is_active());
 }
 
+TEST_F(TestTimeSource, ROS_invalid_sim_time) {
+  rclcpp::TimeSource ts;
+  ts.attachNode(node);
+  EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful);
+}
+
 TEST_F(TestTimeSource, clock) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);