From 203e97c1daf82b91a13ef3065933b7b77fbaa2be Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 27 Jul 2023 19:23:31 +0900 Subject: [PATCH 1/2] refactor(rtc_interafce): add const (#4420) Signed-off-by: satoshi-ota --- .../include/rtc_interface/rtc_interface.hpp | 9 +++++---- planning/rtc_interface/src/rtc_interface.cpp | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 1661ac9ebef3a..4929d0f8e27f3 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -53,8 +53,8 @@ class RTCInterface const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); - bool isActivated(const UUID & uuid); - bool isRegistered(const UUID & uuid); + bool isActivated(const UUID & uuid) const; + bool isRegistered(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); @@ -74,10 +74,9 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_statuses_; rclcpp::Service::SharedPtr srv_commands_; rclcpp::Service::SharedPtr srv_auto_mode_; - - std::mutex mutex_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::Logger logger_; + Module module_; CooperateStatusArray registered_status_; std::vector stored_commands_; @@ -87,6 +86,8 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + + mutable std::mutex mutex_; }; } // namespace rtc_interface diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index f4a95d0acf9d1..dbc82113d403b 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -251,7 +251,7 @@ void RTCInterface::clearCooperateStatus() stored_commands_.clear(); } -bool RTCInterface::isActivated(const UUID & uuid) +bool RTCInterface::isActivated(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( @@ -271,7 +271,7 @@ bool RTCInterface::isActivated(const UUID & uuid) return false; } -bool RTCInterface::isRegistered(const UUID & uuid) +bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( From bea5454960e1547506f1e5ae4b98e419840f95c2 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 29 Nov 2023 11:06:59 +0900 Subject: [PATCH 2/2] 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;