fix both lane change and avoidance by lane change both are running

Signed-off-by: Zulfaqar Azmi <[email protected]>
autowarefoundation · Dec 14, 2023 · 96a94ea · 96a94ea
1 parent b167291
commit 96a94ea
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 10 deletions.
diff --git a/...by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/...by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp
@@ -40,6 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
 
   bool isExecutionRequested() const override;
 
+  void processOnEntry() override;
+
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };

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
@@ -47,6 +47,10 @@ bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
 {
   return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck();
 }
+void AvoidanceByLaneChangeInterface::processOnEntry()
+{
+  waitApproval();
+}
 
 void AvoidanceByLaneChangeInterface::updateRTCStatus(
   const double start_distance, const double finish_distance)

diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -49,18 +49,10 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 {
   const auto & data = avoidance_data_;
 
-  if (!status_.is_safe) {
-    return false;
-  }
-
   if (data.target_objects.empty()) {
     return false;
   }
 
-  if (status_.current_lanes.empty()) {
-    return false;
-  }
-
   const auto & object_parameters = avoidance_parameters_->object_parameters;
 
   const auto count_target_object = [&](const auto sum, const auto & p) {
@@ -82,10 +74,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 
   const auto & front_object = data.target_objects.front();
 
+  const auto current_lanes = getCurrentLanes();
+  if (current_lanes.empty()) {
+    return false;
+  }
   const auto shift_intervals =
-    getRouteHandler()->getLateralIntervalsToPreferredLane(status_.current_lanes.back(), direction_);
+    getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
   const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_, shift_intervals, lane_change_parameters_->backward_length_buffer_for_end_of_lane);
+    *lane_change_parameters_, shift_intervals,
+    lane_change_parameters_->backward_length_buffer_for_end_of_lane);
 
   return (front_object.longitudinal > minimum_lane_change_length);
 }