feat(lane_change): improve execution condition of lane change module (#…

…8648) * refactor lane change utility funcions Signed-off-by: mohammad alqudah <[email protected]> * LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah <[email protected]> * don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah <[email protected]> * modify threshold distance calculation Signed-off-by: mohammad alqudah <[email protected]> * move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah <[email protected]> * always run LC module if approaching terminal point Signed-off-by: mohammad alqudah <[email protected]> * use max possible LC length as threshold Signed-off-by: mohammad alqudah <[email protected]> * update LC readme Signed-off-by: mohammad alqudah <[email protected]> * refactor implementation Signed-off-by: mohammad alqudah <[email protected]> * update readme Signed-off-by: mohammad alqudah <[email protected]> * check distance to reg element for candidate path only if not near terminal start Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]>
autowarefoundation · Sep 18, 2024 · 2f674f8 · 2f674f8
1 parent 18fb465
commit 2f674f8
Show file tree

Hide file tree

Showing 10 changed files with 236 additions and 197 deletions.
diff --git a/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -20,6 +20,7 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp"
 
+#include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
 #include <autoware/behavior_path_lane_change_module/utils/utils.hpp>
 #include <autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
@@ -280,7 +281,7 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
     return std::numeric_limits<double>::infinity();
   }
 
-  return utils::lane_change::calcMinimumLaneChangeLength(
+  return utils::lane_change::calculation::calc_minimum_lane_change_length(
     getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
 }
 

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -11,7 +11,9 @@ The Lane Change module is activated when lane change is needed and can be safely
   - `allow_lane_change` tags is set as `true`
 - During lane change request condition
   - The ego-vehicle isn’t on a `preferred_lane`.
-  - There is neither intersection nor crosswalk on the path of the lane change
+  - The ego-vehicle isn't approaching a traffic light. (condition parameterized)
+  - The ego-vehicle isn't approaching a crosswalk. (condition parameterized)
+  - The ego-vehicle isn't approaching an intersection. (condition parameterized)
 - lane change ready condition
   - Path of the lane change does not collide with other dynamic objects (see the figure below)
   - Lane change candidate path is approved by an operator.
@@ -182,11 +184,9 @@ A candidate path is considered valid if it meets the following criteria:
 1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change.
 2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes.
 3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes.
-4. Intersection requirements are met (conditions are parameterized).
-5. Crosswalk requirements are satisfied (conditions are parameterized).
-6. Traffic light regulations are adhered to (conditions are parameterized).
-7. The lane change can be completed after passing a parked vehicle.
-8. The lane change is deemed safe to execute.
+4. The distance from the ego vehicle's current position to the next regulatory element is adequate to perform a single lane change.
+5. The lane change can be completed after passing a parked vehicle.
+6. The lane change is deemed safe to execute.
 
 The following flow chart illustrates the validity check.
 
@@ -231,43 +231,6 @@ group check for distance #LightYellow
     endif
 end group
 
-
-
-group evaluate on Crosswalk #LightCyan
-if (regulate_on_crosswalk and not enough length to crosswalk) then (yes)
-  if (stop time < stop time threshold\n(Related to stuck detection)) then (yes)
-    #LightPink:Reject path;
-    stop
-  else (no)
-    :Allow lane change in crosswalk;
-  endif
-else (no)
-endif
-end group
-
-group evaluate on Intersection #LightGreen
-if (regulate_on_intersection and not enough length to intersection) then (yes)
-  if (stop time < stop time threshold\n(Related to stuck detection)) then (yes)
-    #LightPink:Reject path;
-    stop
-  else (no)
-    :Allow lane change in intersection;
-  endif
-else (no)
-endif
-end group
-
-group evaluate on Traffic Light #Lavender
-if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes)
-  #LightPink:Reject path;
-  stop
-elseif (stopped at red traffic light within distance) then (yes)
-  #LightPink:Reject path;
-  stop
-else (no)
-endif
-end group
-
 if (ego is not stuck but parked vehicle exists in target lane) then (yes)
   #LightPink:Reject path;
   stop
@@ -517,8 +480,8 @@ The function to stop by keeping a margin against forward obstacle in the previou
 
 ### Lane change regulations
 
-If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections.
-To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`.
+If you want to regulate lane change on crosswalks, intersections, or traffic lights, the lane change module is disabled near any of them.
+To regulate lane change on crosswalks, intersections, or traffic lights, set `regulation.crosswalk`, `regulation.intersection` or `regulation.traffic_light` to `true`.
 If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection.
 If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck.
 If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping.

diff --git a/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
@@ -109,6 +109,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isStoppedAtRedTrafficLight() const override;
 
+  bool is_near_regulatory_element() const final;
+
   TurnSignalInfo get_current_turn_signal_info() const final;
 
 protected:

diff --git a/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
@@ -231,6 +231,8 @@ class LaneChangeBase
 
   virtual TurnSignalInfo get_current_turn_signal_info() const = 0;
 
+  virtual bool is_near_regulatory_element() const = 0;
+
 protected:
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
 

diff --git a/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
@@ -16,8 +16,12 @@
 
 #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp"
 
+#include <autoware/route_handler/route_handler.hpp>
+
 namespace autoware::behavior_path_planner::utils::lane_change::calculation
 {
+using autoware::route_handler::Direction;
+using autoware::route_handler::RouteHandler;
 using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LCParamPtr;
 
@@ -113,6 +117,21 @@ double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr);
 double calc_ego_dist_to_lanes_start(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
+
+double calc_minimum_lane_change_length(
+  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);
+
+double calc_minimum_lane_change_length(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
+  const LaneChangeParameters & lane_change_parameters, Direction direction);
+
+double calc_maximum_lane_change_length(
+  const double current_velocity, const LaneChangeParameters & lane_change_parameters,
+  const std::vector<double> & shift_intervals, const double max_acc);
+
+double calc_maximum_lane_change_length(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
+  const double max_acc);
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -60,17 +60,6 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 double calcLaneChangeResampleInterval(
   const double lane_changing_length, const double lane_changing_velocity);
 
-double calcMinimumLaneChangeLength(
-  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);
-
-double calcMinimumLaneChangeLength(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
-  const LaneChangeParameters & lane_change_parameters, Direction direction);
-
-double calcMaximumLaneChangeLength(
-  const double current_velocity, const LaneChangeParameters & lane_change_parameters,
-  const std::vector<double> & shift_intervals, const double max_acc);
-
 double calcMinimumAcceleration(
   const double current_velocity, const double min_longitudinal_acc,
   const LaneChangeParameters & lane_change_parameters);
@@ -305,6 +294,10 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co
 ExtendedPredictedObjects transform_to_extended_objects(
   const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
   const bool check_prepare_phase);
+
+double get_distance_to_next_regulatory_element(
+  const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
+  const bool ignore_intersection = false);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -147,6 +147,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   *prev_approved_path_ = getPreviousModuleOutput().path;
 
   BehaviorModuleOutput out = getPreviousModuleOutput();
+
   module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path);
   out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
@@ -249,6 +250,13 @@ bool LaneChangeInterface::canTransitFailureState()
   }
 
   if (isWaitingApproval()) {
+    if (module_type_->is_near_regulatory_element()) {
+      log_debug_throttled("Ego is close to regulatory element. Cancel lane change");
+      updateRTCStatus(
+        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+        State::FAILED);
+      return true;
+    }
     log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL");
     return false;
   }