From 1b758367072b1e173e9acb2e6296d48ec984b6ae Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Thu, 13 Aug 2020 16:14:04 -0400 Subject: [PATCH] Fix clock thread issue (#1266) (#1267) * lock before rcl_set_ros_time_override Signed-off-by: Daisuke Sato Signed-off-by: Tomoya Fujita --- 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 74bf38e894..0a4fc4275a 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -148,8 +148,6 @@ class TimeSource 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 fd1da9c9f7..8cbf6bbf33 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_) {