From b6518dd559140b549c2b6651666e97e7f5410481 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 13 May 2024 13:54:31 +0900 Subject: [PATCH] feat(rtc_interface)!: add new field to rtc cooperate status (#6933) * feat(rtc_interface): add new field Signed-off-by: satoshi-ota * feat(bvp): support new rtc cooperate status Signed-off-by: satoshi-ota * feat(bpp): support new rtc cooperate status Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/interface.cpp | 5 ++++- .../include/behavior_path_avoidance_module/scene.hpp | 10 ++++++---- .../behavior_path_avoidance_module/type_alias.hpp | 2 ++ .../interface/scene_module_interface.hpp | 5 ++++- .../src/manager.cpp | 3 ++- .../src/manager.cpp | 4 ++-- .../src/manager.cpp | 3 ++- .../src/manager.cpp | 11 ++++++----- .../src/manager.cpp | 3 ++- .../scene_module_interface.hpp | 7 +++++-- .../src/scene_module_interface.cpp | 3 ++- .../src/manager.cpp | 3 ++- .../include/rtc_interface/rtc_interface.hpp | 6 ++++-- planning/rtc_interface/src/rtc_interface.cpp | 6 ++++-- 14 files changed, 47 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3a7c22c84b3e8..a2e26557d9726 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -57,7 +57,10 @@ void AvoidanceByLaneChangeInterface::updateRTCStatus( return (dir == Direction::LEFT) ? "left" : "right"; }); + const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING; + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); + uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance, + clock_->now()); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 894cd9d5f1c34..8ddff3a38dfb4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -77,15 +77,17 @@ class AvoidanceModule : public SceneModuleInterface { if (candidate.lateral_shift > 0.0) { rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); + uuid_map_.at("left"), isExecutionReady(), State::WAITING_FOR_EXECUTION, + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + clock_->now()); candidate_uuid_ = uuid_map_.at("left"); return; } if (candidate.lateral_shift < 0.0) { rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); + uuid_map_.at("right"), isExecutionReady(), State::WAITING_FOR_EXECUTION, + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + clock_->now()); candidate_uuid_ = uuid_map_.at("right"); return; } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp index 8e7c7820a3650..67bca3099df6f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -49,6 +50,7 @@ using visualization_msgs::msg::MarkerArray; // tier4 msgs using tier4_planning_msgs::msg::AvoidanceDebugMsg; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_rtc_msgs::msg::State; // tier4 utils functions using tier4_autoware_utils::appendMarkerArray; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 4947a1b544302..321b193ca187b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; +using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; @@ -503,8 +505,9 @@ class SceneModuleInterface { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { + const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING; ptr->updateCooperateStatus( - uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance, + uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance, clock_->now()); } } diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 3651833b8ef4f..07742a1217ab3 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -79,7 +79,8 @@ void BlindSpotModuleManager::launchNewModules( logger_.get_child("blind_spot_module"), clock_)); generateUUID(module_id); updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + path.header.stamp); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index ad56df5f76944..5110ff9993a62 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -190,8 +190,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) clock_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( - getUUID(crosswalk_lanelet_id), true, std::numeric_limits::lowest(), - path.header.stamp); + getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); }; const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 834ffc03e5dde..8e9a6b6a58a96 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -66,7 +66,8 @@ void DetectionAreaModuleManager::launchNewModules( logger_.get_child("detection_area_module"), clock_)); generateUUID(module_id); updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(module_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); } } } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3941362d96242..424fb6841eca9 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -345,10 +345,10 @@ void IntersectionModuleManager::launchNewModules( const UUID uuid = getUUID(new_module->getModuleId()); const auto occlusion_uuid = new_module->getOcclusionUUID(); rtc_interface_.updateCooperateStatus( - uuid, true, std::numeric_limits::lowest(), std::numeric_limits::lowest(), - clock_->now()); + uuid, true, State::RUNNING, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, true, std::numeric_limits::lowest(), + occlusion_uuid, true, State::RUNNING, std::numeric_limits::lowest(), std::numeric_limits::lowest(), clock_->now()); registerModule(std::move(new_module)); } @@ -404,12 +404,13 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) const UUID uuid = getUUID(scene_module->getModuleId()); const bool safety = scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); - updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp); + updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); const auto occlusion_distance = intersection_module->getOcclusionDistance(); const auto occlusion_safety = intersection_module->getOcclusionSafety(); occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance, + stamp); // ========================================================================================== // module debug data diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index c4192750af1d5..46cc224ea0f6b 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -68,7 +68,8 @@ void NoStoppingAreaModuleManager::launchNewModules( clock_)); generateUUID(module_id); updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(module_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); } } } 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 d8b042ec880e4..082a88e306169 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 @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -58,6 +59,7 @@ using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -235,9 +237,10 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface virtual void setActivation(); void updateRTCStatus( - const UUID & uuid, const bool safe, const double distance, const Time & stamp) + const UUID & uuid, const bool safe, const uint8_t state, const double distance, + const Time & stamp) { - rtc_interface_.updateCooperateStatus(uuid, safe, distance, distance, stamp); + rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); } void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 362493005eb17..803682489ebde 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -211,7 +211,8 @@ void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) { for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + const auto state = scene_module->isActivated() ? State::RUNNING : State::WAITING_FOR_EXECUTION; + updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); } publishRTCStatus(stamp); } diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index bdc7f04360ff0..2c14f7489832a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -132,7 +132,8 @@ void TrafficLightModuleManager::launchNewModules( logger_.get_child("traffic_light_module"), clock_)); generateUUID(lane_id); updateRTCStatus( - getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); } } } diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index cbccc0b99262d..8a97b2b643987 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -24,6 +24,7 @@ #include "tier4_rtc_msgs/msg/cooperate_status.hpp" #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp" #include "tier4_rtc_msgs/msg/module.hpp" +#include "tier4_rtc_msgs/msg/state.hpp" #include "tier4_rtc_msgs/srv/auto_mode.hpp" #include "tier4_rtc_msgs/srv/cooperate_commands.hpp" #include @@ -41,6 +42,7 @@ using tier4_rtc_msgs::msg::CooperateResponse; using tier4_rtc_msgs::msg::CooperateStatus; using tier4_rtc_msgs::msg::CooperateStatusArray; using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; using tier4_rtc_msgs::srv::AutoMode; using tier4_rtc_msgs::srv::CooperateCommands; using unique_identifier_msgs::msg::UUID; @@ -51,8 +53,8 @@ class RTCInterface RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true); void publishCooperateStatus(const rclcpp::Time & stamp); void updateCooperateStatus( - const UUID & uuid, const bool safe, const double start_distance, const double finish_distance, - const rclcpp::Time & stamp); + const UUID & uuid, const bool safe, const uint8_t state, const double start_distance, + const double finish_distance, const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 2e470f3360d34..9e3e810b68e17 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -201,8 +201,8 @@ void RTCInterface::onTimer() } void RTCInterface::updateCooperateStatus( - const UUID & uuid, const bool safe, const double start_distance, const double finish_distance, - const rclcpp::Time & stamp) + const UUID & uuid, const bool safe, const uint8_t state, const double start_distance, + const double finish_distance, const rclcpp::Time & stamp) { std::lock_guard lock(mutex_); // Find registered status which has same uuid @@ -218,6 +218,7 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; + status.state.type = State::WAITING_FOR_EXECUTION; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; @@ -228,6 +229,7 @@ void RTCInterface::updateCooperateStatus( // If the registered status is found, update status itr->stamp = stamp; itr->safe = safe; + itr->state.type = state; itr->start_distance = start_distance; itr->finish_distance = finish_distance; }