Merge pull request #1300 from tier4/cp-lane-change

fix(lane_change): cherry-pick several lane change commits
tier4 · May 20, 2024 · 9bae39d · 9bae39d
2 parents 3ff7151 + 60f91a2
commit 9bae39d
Show file tree

Hide file tree

Showing 26 changed files with 986 additions and 523 deletions.
diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml
@@ -51,10 +51,14 @@ Planning:
   behavior_path_avoidance_by_lane_change:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: lane_change.utils
 
   behavior_path_lane_change:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: lane_change.NORMAL
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: lane_change.utils
 
   behavior_velocity_planner:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner

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
@@ -125,7 +125,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   // reference pose
   data.reference_pose = getEgoPose();
 
-  data.reference_path_rough = prev_module_path_;
+  data.reference_path_rough = prev_module_output_.path;
 
   const auto resample_interval = avoidance_parameters_->resample_interval_for_planning;
   data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval);
@@ -145,7 +145,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *planner_data_->dynamic_object, data.current_lanelets,
-      utils::path_safety_checker::isPolygonOverlapLanelet);
+      [](const auto & obj, const auto & lane) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      });
 
   // Assume that the maximum allocation for data.other object is the sum of
   // objects_within_target_lane and object_outside_target_lane. The maximum allocation for
@@ -273,12 +275,8 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
     return std::numeric_limits<double>::infinity();
   }
 
-  // get minimum lane change distance
-  const auto shift_intervals =
-    getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
   return utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_, shift_intervals,
-    lane_change_parameters_->backward_length_buffer_for_end_of_lane);
+    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp
@@ -145,7 +145,9 @@ PredictedObjects extractObjectsInExpandedPullOverLanes(
     route_handler, left_side, backward_distance, forward_distance, bound_offset);
 
   const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
-    objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+    objects, lanes, [](const auto & obj, const auto & lanelet) {
+      return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet);
+    });
 
   return objects_in_lanes;
 }

diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md
@@ -319,6 +319,171 @@ The detection area for the target lane can be expanded beyond its original bound
   </table>
 </div>
 
+##### Object filtering
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+title NormalLaneChange::filterObjects Method Execution Flow
+
+start
+
+group "Filter Objects by Class" {
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  if (current object type != param.object_types_to_check?) then (TRUE)
+  #LightPink:Remove current object;
+else (FALSE)
+  :Keep current object;
+endif
+end while
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Oncoming Objects" #PowderBlue {
+:Iterate through each object in target lane objects list;
+while (has not finished iterating through object list?) is (TRUE)
+:check object's yaw with reference to ego's yaw.;
+if (yaw difference < 90 degree?) then (TRUE)
+  :Keep current object;
+else (FALSE)
+if (object is stopping?) then (TRUE)
+  :Keep current object;
+else (FALSE)
+  #LightPink:Remove current object;
+endif
+endif
+endwhile
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Objects Ahead Terminal" #Beige {
+:Calculate lateral distance from ego to current lanes center;
+
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :Get current object's polygon;
+  :Initialize distance to terminal from object to max;
+  while (has not finished iterating through object polygon's vertices) is (TRUE)
+    :Calculate object's lateral distance to end of lane;
+    :Update minimum distance to terminal from object;
+  end while
+  if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
+      #LightPink:Remove current object;
+  else (FALSE)
+  endif
+end while
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Objects By Lanelets" #LightGreen {
+
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
+  if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
+    :Add to target_lane_objects;
+    else (FALSE)
+      if (Object overlaps with backward target lanes?) then (TRUE)
+        :Add to target_lane_objects;
+      else (FALSE)
+        if (Object in current lane polygon?) then (TRUE)
+          :Add to current_lane_objects;
+        else (FALSE)
+          :Add to other_lane_objects;
+        endif
+      endif
+  endif
+end while
+
+:Return target lanes object,  current lanes object and other lanes object;
+end group
+
+:Generate path from current lanes;
+
+if (path empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Target Lanes' objects" #LightCyan {
+
+:Iterate through each object in target lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :Keep current object;
+  else (FALSE)
+    :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+   endif
+endwhile
+end group
+
+group "Filter Current Lanes' objects"  #LightYellow {
+
+:Iterate through each object in current lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+    else (FALSE)
+      #LightPink:remove current object;
+   endif
+endwhile
+end group
+
+group "Filter Other Lanes' objects"  #Lavender {
+
+:Iterate through each object in other lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+    else (FALSE)
+      #LightPink:remove current object;
+   endif
+endwhile
+end group
+
+:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
+stop
+
+@enduml
+```
+
 ##### Collision check in prepare phase
 
 The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.
@@ -551,13 +716,15 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 
 #### Additional parameters
 
-| Name                                       | Unit  | Type    | Description                                                                                                                                                | Default value |
-| :----------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
-| `enable_collision_check_at_prepare_phase`  | [-]   | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase.                                   | false         |
-| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double  | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1           |
-| `check_objects_on_current_lanes`           | [-]   | boolean | If true, the lane change module check objects on current lanes when performing collision assessment.                                                       | false         |
-| `check_objects_on_other_lanes`             | [-]   | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment                                                       | false         |
-| `use_all_predicted_path`                   | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                     | true          |
+| Name                                                     | Unit  | Type    | Description                                                                                                                                                                                                | Default value |
+| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `enable_collision_check_for_prepare_phase.general_lanes` | [-]   | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false         |
+| `enable_collision_check_for_prepare_phase.intersection`  | [-]   | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase.                                                       | true          |
+| `enable_collision_check_for_prepare_phase.turns`         | [-]   | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase.                                   | true          |
+| `prepare_phase_ignore_target_speed_thresh`               | [m/s] | double  | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true`                                                 | 0.1           |
+| `check_objects_on_current_lanes`                         | [-]   | boolean | If true, the lane change module check objects on current lanes when performing collision assessment.                                                                                                       | false         |
+| `check_objects_on_other_lanes`                           | [-]   | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment                                                                                                       | false         |
+| `use_all_predicted_path`                                 | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                                                                     | true          |
 
 #### safety constraints during lane change path is computed
 

diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -89,7 +89,10 @@
         stop_time: 3.0 # [s]
 
       # collision check
-      enable_prepare_segment_collision_check: true
+      enable_collision_check_for_prepare_phase:
+        general_lanes: false
+        intersection: true
+        turns: true
       prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]
       check_objects_on_current_lanes: true
       check_objects_on_other_lanes: true

diff --git a/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp
@@ -93,9 +93,6 @@ class LaneChangeInterface : public SceneModuleInterface
 
   MarkerArray getModuleVirtualWall() override;
 
-  TurnSignalInfo getCurrentTurnSignalInfo(
-    const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info);
-
   // TODO(someone): remove this, and use base class function
   [[deprecated]] BehaviorModuleOutput run() override
   {

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
@@ -30,10 +30,12 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
+using data::lane_change::LanesPolygon;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::Direction;
+using utils::path_safety_checker::ExtendedPredictedObjects;
 
 class NormalLaneChange : public LaneChangeBase
 {
@@ -99,6 +101,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isStoppedAtRedTrafficLight() const override;
 
+  TurnSignalInfo get_current_turn_signal_info() override;
+
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 
@@ -115,10 +119,26 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  LaneChangeTargetObjects getTargetObjects(
+  ExtendedPredictedObjects getTargetObjects(
+    const LaneChangeLanesFilteredObjects & predicted_objects,
+    const lanelet::ConstLanelets & current_lanes) const;
+
+  LaneChangeLanesFilteredObjects filterObjects(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
+  void filterOncomingObjects(PredictedObjects & objects) const;
+
+  void filterAheadTerminalObjects(
+    PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
+
+  void filterObjectsByLanelets(
+    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes,
+    std::vector<PredictedObject> & current_lane_objects,
+    std::vector<PredictedObject> & target_lane_objects,
+    std::vector<PredictedObject> & other_lane_objects) const;
+
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
     const double prepare_length) const override;
@@ -154,26 +174,23 @@ class NormalLaneChange : public LaneChangeBase
   bool isValidPath(const PathWithLaneId & path) const override;
 
   PathSafetyStatus isLaneChangePathSafe(
-    const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
+    const LaneChangePath & lane_change_path,
+    const ExtendedPredictedObjects & collision_check_objects,
+    const utils::path_safety_checker::RSSparams & rss_params,
     CollisionCheckDebugMap & debug_data) const;
 
-  LaneChangeTargetObjectIndices filterObject(
-    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes,
-    const lanelet::ConstLanelets & target_backward_lanes) const;
-
-  std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
-    const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;
-
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.
   bool isVehicleStuck(
     const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const;
 
+  double get_max_velocity_for_safety_check() const;
+
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
+  bool check_prepare_phase() const;
+
   double calcMaximumLaneChangeLength(
     const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;