From 1fff1b7cbafcbc698b5dda5b8e4a35a27c6748c4 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 9 Jun 2021 06:07:15 +0900 Subject: [PATCH] Fix clock thread issue (#1266) (#1267) (#1685) Signed-off-by: Daisuke Sato Signed-off-by: Tomoya Fujita Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> --- rclcpp/include/rclcpp/time_source.hpp | 2 -- rclcpp/src/rclcpp/time_source.cpp | 32 ++++++++++----------------- 2 files changed, 12 insertions(+), 22 deletions(-) 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_) {