diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 74bf38e..0a4fc42 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -148,8 +148,6 @@ private: void disable_ros_time(); // Internal helper functions used inside iterators - static void enable_ros_time(rclcpp::Clock::SharedPtr clock); - static void disable_ros_time(rclcpp::Clock::SharedPtr clock); static void set_clock( const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index fd1da9c..8cbf6bb 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -185,11 +185,21 @@ void TimeSource::set_clock( const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, std::shared_ptr clock) { + std::lock_guard clock_guard(clock->get_clock_mutex()); + // Do change if (!set_ros_time_enabled && clock->ros_time_is_active()) { - disable_ros_time(clock); + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to disable ros_time_override_status"); + } } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { - enable_ros_time(clock); + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } } auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds()); @@ -273,24 +283,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S } } -void TimeSource::enable_ros_time(std::shared_ptr clock) -{ - auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); - } -} - -void TimeSource::disable_ros_time(std::shared_ptr clock) -{ - auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); - } -} - void TimeSource::enable_ros_time() { if (ros_time_active_) {