fix(behavior_path_planner): fix RTC status update in avoidance module…

… with multiple shift point case (autowarefoundation#1295) Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
tzhong518 · Oct 14, 2022 · 7b40237 · 7b40237
1 parent 74c3110
commit 7b40237
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 8 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_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
@@ -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<double>::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();