From a792d4b298068b13830a0895700801b8679c44d4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 21 May 2024 18:29:18 +0900 Subject: [PATCH] fix(rtc_interface): use clock created by node to judge expired cooperate status (#7083) Signed-off-by: satoshi-ota --- .../rtc_interface/include/rtc_interface/rtc_interface.hpp | 1 + planning/rtc_interface/src/rtc_interface.cpp | 7 +++---- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 7180812c7500b..7ab796321e041 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -84,6 +84,7 @@ class RTCInterface rclcpp::Service::SharedPtr srv_auto_mode_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; Module module_; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 623f899f55c70..5e4b202071e49 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -81,7 +81,8 @@ Module getModuleType(const std::string & module_name) namespace rtc_interface { RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc) -: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, +: clock_{node->get_clock()}, + logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, is_auto_mode_enabled_{!enable_rtc}, is_locked_{false} { @@ -270,9 +271,7 @@ void RTCInterface::removeExpiredCooperateStatus() std::lock_guard lock(mutex_); const auto itr = std::remove_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), - [](const auto & status) { - return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0; - }); + [this](const auto & status) { return (clock_->now() - status.stamp).seconds() > 10.0; }); registered_status_.statuses.erase(itr, registered_status_.statuses.end()); }