diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 465ceaf5a7..c6e39ace70 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -288,10 +288,10 @@ class TimeSource::NodeState final // Detach the attached node void detachNode() { - std::lock_guard guard(node_base_lock_); // destroy_clock_sub() *must* be first here, to ensure that the executor // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); + std::lock_guard guard(node_base_lock_); clocks_state_.disable_ros_time(); if (on_set_parameters_callback_) { node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get()); @@ -409,9 +409,14 @@ class TimeSource::NodeState final "/clock", qos_, [this](std::shared_ptr msg) { - // We are using node_base_ as an indication if there is a node attached. - // Only call the clock_cb if that is the case. - if (node_base_ != nullptr) { + bool execute_cb = false; + { + std::lock_guard guard(node_base_lock_); + // We are using node_base_ as an indication if there is a node attached. + // Only call the clock_cb if that is the case. + execute_cb = node_base_ != nullptr; + } + if (execute_cb) { clock_cb(msg); } }, diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index 7fd1676c5d..f6bbd2ccc8 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -54,8 +54,16 @@ class TimerNode : public rclcpp::Node std::bind(&TimerNode::Timer2Callback, this)); } - int GetTimer1Cnt() {return cnt1_;} - int GetTimer2Cnt() {return cnt2_;} + int GetTimer1Cnt() + { + const std::lock_guard lock(mutex_); + return cnt1_; + } + int GetTimer2Cnt() + { + const std::lock_guard lock(mutex_); + return cnt2_; + } void ResetTimer1() { @@ -82,16 +90,24 @@ class TimerNode : public rclcpp::Node private: void Timer1Callback() { - cnt1_++; + { + const std::lock_guard lock(mutex_); + cnt1_++; + } RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); } void Timer2Callback() { - cnt2_++; + { + const std::lock_guard lock(mutex_); + cnt2_++; + } RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); } + std::mutex mutex_; + rclcpp::TimerBase::SharedPtr timer1_; rclcpp::TimerBase::SharedPtr timer2_; int cnt1_ = 0; @@ -130,6 +146,18 @@ class ClockPublisher : public rclcpp::Node } } + bool wait_for_connection(std::chrono::nanoseconds timeout) + { + auto end_time = std::chrono::steady_clock::now() + timeout; + while (clock_publisher_->get_subscription_count() == 0 && + (std::chrono::steady_clock::now() < end_time)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + return clock_publisher_->get_subscription_count() != 0; + } + void sleep_for(rclcpp::Duration duration) { rclcpp::Time start_time(0, 0, RCL_ROS_TIME); @@ -148,7 +176,10 @@ class ClockPublisher : public rclcpp::Node return; } std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - rostime_ += ros_update_duration_; + { + const std::lock_guard lock(mutex_); + rostime_ += ros_update_duration_; + } } } @@ -163,9 +194,11 @@ class ClockPublisher : public rclcpp::Node void PublishClock() { - const std::lock_guard lock(mutex_); auto message = rosgraph_msgs::msg::Clock(); - message.clock = rostime_; + { + const std::lock_guard lock(mutex_); + message.clock = rostime_; + } clock_publisher_->publish(message); } @@ -227,6 +260,9 @@ class TestTimerCancelBehavior : public ::testing::Test [this]() { executor.spin(); }); + + EXPECT_TRUE(this->sim_clock_node->wait_for_connection(50ms)); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->ros_time_is_active()); } void TearDown()