RT0-33893 enable cancel when ego in turn direction lane

Signed-off-by: Zulfaqar Azmi <[email protected]>
tier4 · Oct 21, 2024 · 6608650 · 6608650
1 parent a7e68a1
commit 6608650
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 1 deletion.
diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
@@ -86,6 +86,18 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbleToReturnCurrentLane() const override;
 
+  /**
+   * @brief Determines if ego vehicle is within lanes designated for turning.
+   *
+   * Checks if ego's polygon overlaps with lanelets tagged for turning directions (excluding
+   * 'straight'). It evaluates the lanelet's 'turn_direction' attribute and determines overlap with
+   * the lanelet's area.
+   *
+   * @return bool True if the ego's polygon is within a lane designated for turning, false if it is
+   * within a straight lane or no turn direction is specified.
+   */
+  bool is_within_turn_direction_lanes() const final;
+
   bool isEgoOnPreparePhase() const override;
 
   bool isAbleToStopSafely() const override;

diff --git a/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp
@@ -86,6 +86,8 @@ class LaneChangeBase
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
+  virtual bool is_within_turn_direction_lanes() const = 0;
+
   virtual LaneChangePath getLaneChangePath() const = 0;
 
   virtual bool isEgoOnPreparePhase() const = 0;

diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -141,10 +141,12 @@ ModuleStatus LaneChangeInterface::updateState()
     return ModuleStatus::RUNNING;
   }
 
+  const auto within_turn_direction_lane = module_type_->is_within_turn_direction_lanes();
+
   if (!module_type_->isAbleToReturnCurrentLane()) {
     log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change.");
     change_state_if_stop_required();
-    return ModuleStatus::RUNNING;
+    return within_turn_direction_lane ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
   }
 
   const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane;

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -566,6 +566,22 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const
   return true;
 }
 
+bool NormalLaneChange::is_within_turn_direction_lanes() const
+{
+  const auto & route_handler_ptr = planner_data_->route_handler;
+  const auto & ego_pose = getEgoPose();
+
+  lanelet::ConstLanelet current_lane;
+  if (!route_handler_ptr->getClosestLaneletWithinRoute(ego_pose, &current_lane)) {
+    return false;
+  }
+
+  const auto ego_polygon =
+    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info);
+
+  return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_polygon);
+};
+
 bool NormalLaneChange::isEgoOnPreparePhase() const
 {
   const auto & start_position = status_.lane_change_path.info.shift_line.start.position;