From 01c7061e1b87c38c7beaf6d17d39e552a69e07b8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 8 Jul 2022 20:48:08 +0900 Subject: [PATCH] fix(behavior_path_planner): fix RTC status update in avoidance module with multiple shift point case Signed-off-by: Fumiya Watanabe --- .../scene_module/avoidance/avoidance_module.hpp | 14 ++++++++++++++ .../scene_module/avoidance/avoidance_module.cpp | 15 +++++++-------- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index c5a0a0f78b63e..bcdb36a92827a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -107,6 +107,20 @@ class AvoidanceModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } + void removePreviousRTCStatusLeft() + { + if (rtc_interface_left_.isRegistered(uuid_left_)) { + rtc_interface_left_.removeCooperateStatus(uuid_left_); + } + } + + void removePreviousRTCStatusRight() + { + if (rtc_interface_right_.isRegistered(uuid_right_)) { + rtc_interface_right_.removeCooperateStatus(uuid_right_); + } + } + // data used in previous planning ShiftedPath prev_output_; ShiftedPath prev_linear_shift_path_; // used for shift point check diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 85dc43e205f66..a85bf8613075f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2096,6 +2096,13 @@ BehaviorModuleOutput AvoidanceModule::plan() debug_data_.new_shift_points = *new_shift_points; DEBUG_PRINT("new_shift_points size = %lu", new_shift_points->size()); printShiftPoints(*new_shift_points, "new_shift_points"); + if (new_shift_points->back().getRelativeLength() > 0.0) { + removePreviousRTCStatusRight(); + } else if (new_shift_points->back().getRelativeLength() < 0.0) { + removePreviousRTCStatusLeft(); + } else { + RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); + } addShiftPointIfApproved(*new_shift_points); } else if (isWaitingApproval()) { clearWaitingApproval(); @@ -2189,14 +2196,6 @@ void AvoidanceModule::addShiftPointIfApproved(const AvoidPointArray & shift_poin // register original points for consistency registerRawShiftPoints(shift_points); - TurnSignalInfo turn_signal_info; - turn_signal_info.signal_distance = std::numeric_limits::lowest(); - if (shift_points.back().getRelativeLength() > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else if (shift_points.back().getRelativeLength() < 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - uuid_left_ = generateUUID(); uuid_right_ = generateUUID();