diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 4dc46ce..9dd4023 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/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 @@ private: std::mutex clock_list_lock_; // A vector to store references to associated clocks. std::vector associated_clocks_; + // A handler for the use_sim_time parameter callback. + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index bde1c50..4d74d70 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/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 & 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(); diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index bd8a186..ef624bd 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/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(RCL_ROS_TIME);