From bea5454960e1547506f1e5ae4b98e419840f95c2 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 29 Nov 2023 11:06:59 +0900 Subject: [PATCH] fix(behavior_velocity_planner_common): sync activation when safety status is set (#5697) * fix(behavior_velocity_planner_common): sync activation when safety status is set Signed-off-by: Fumiya Watanabe * Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp Co-authored-by: Kyoichi Sugahara * Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp Co-authored-by: Tomoya Kimura --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Kyoichi Sugahara Co-authored-by: Tomoya Kimura --- .../scene_module_interface.hpp | 12 +++++++++++- .../include/rtc_interface/rtc_interface.hpp | 1 + planning/rtc_interface/src/rtc_interface.cpp | 16 ++++++++++++++++ 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index c33ce84ff09de..63e72ca78ed62 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -101,6 +101,7 @@ class SceneModuleInterface boost::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } bool isActivated() const { return activated_; } bool isSafe() const { return safe_; } double getDistance() const { return distance_; } @@ -112,6 +113,7 @@ class SceneModuleInterface const int64_t module_id_; bool activated_; bool safe_; + bool rtc_enabled_; double distance_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; @@ -120,8 +122,15 @@ class SceneModuleInterface boost::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; - void setSafe(const bool safe) { safe_ = safe; } + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } template size_t findEgoSegmentIndex(const std::vector & points) const @@ -357,6 +366,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); scene_module->setActivation(rtc_interface_.isActivated(uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); } } diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..af2c1a3fd0e9b 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -55,6 +55,7 @@ class RTCInterface void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; + bool isRTCEnabled(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index dbc82113d403b..ae2257785f33a 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -280,6 +280,22 @@ bool RTCInterface::isRegistered(const UUID & uuid) const return itr != registered_status_.statuses.end(); } +bool RTCInterface::isRTCEnabled(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + return !itr->auto_mode; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl); + return is_auto_mode_init_; +} + void RTCInterface::lockCommandUpdate() { is_locked_ = true;