feat(behavior_path_planner): update behavior path planner

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -952,7 +952,10 @@ calcArcLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
 template <class T>
 std::vector<double> calcCurvature(const T & points)
 {
-  std::vector<double> curvature_vec(points.size());
+  std::vector<double> curvature_vec(points.size(), 0.0);
+  if (points.size() < 3) {
+    return curvature_vec;
+  }
 
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
 }
 
 void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
-  AvoidancePlanningData & data, DebugData & debug) const
+  AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
@@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
       [&](const auto & object) { return createObjectData(data, object); });
   }
 
-  utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p);
+  utils::avoidance::filterTargetObjects(
+    target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance,
+    planner_data_, p);
 }
 
 ObjectData AvoidanceByLaneChange::createObjectData(

planning/behavior_path_avoidance_module/README.md

@@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.`
 | object_ignore_section_crosswalk_behind_distance       | [m]  | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it.                                                                                                                            | 30.0          |
 | object_check_forward_distance                         | [m]  | double | Forward distance to search the avoidance target.                                                                                                                                                                                       | 150.0         |
 | object_check_backward_distance                        | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 2.0           |
-| object_check_goal_distance                            | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 20.0          |
+| object_check_goal_distance                            | [m]  | double | If the distance between object and goal position is less than this parameter, the module do not return center line.                                                                                                                    | 20.0          |
+| object_check_return_pose_distance                     | [m]  | double | If the distance between object and return position is less than this parameter, the module do not return center line.                                                                                                                  | 20.0          |
 | object_check_shiftable_ratio                          | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.6           |
 | object_check_min_road_shoulder_width                  | [m]  | double | Width considered as a road shoulder if the lane does not have a road shoulder target.                                                                                                                                                  | 0.5           |
 | object_last_seen_threshold                            | [s]  | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0           |

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -122,7 +122,8 @@
           motorcycle: true                               # [-]
           pedestrian: true                               # [-]
         # detection range
-        object_check_goal_distance: 20.0                # [m]
+        object_check_goal_distance: 20.0                 # [m]
+        object_check_return_pose_distance: 20.0          # [m]
         # filtering parking objects
         threshold_distance_object_is_on_center: 1.0     # [m]
         object_check_shiftable_ratio: 0.6               # [-]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -164,10 +164,14 @@ struct AvoidanceParameters
   double object_check_backward_distance{0.0};
   double object_check_yaw_deviation{0.0};
 
-  // if the distance between object and goal position is less than this parameter, the module ignore
-  // the object.
+  // if the distance between object and goal position is less than this parameter, the module do not
+  // return center line.
   double object_check_goal_distance{0.0};
 
+  // if the distance between object and return position is less than this parameter, the module do
+  // not return center line.
+  double object_check_return_pose_distance{0.0};
+
   // use in judge whether the vehicle is parking object on road shoulder
   double object_check_shiftable_ratio{0.0};
 
@@ -462,14 +466,14 @@ using AvoidLineArray = std::vector<AvoidLine>;
 
 struct AvoidOutline
 {
-  AvoidOutline(AvoidLine avoid_line, AvoidLine return_line)
+  AvoidOutline(AvoidLine avoid_line, const std::optional<AvoidLine> return_line)
   : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)}
   {
   }
 
   AvoidLine avoid_line{};
 
-  AvoidLine return_line{};
+  std::optional<AvoidLine> return_line{};
 
   AvoidLineArray middle_lines{};
 };
@@ -588,7 +592,7 @@ struct ShiftLineData
  */
 struct DebugData
 {
-  geometry_msgs::msg::Polygon detection_area;
+  std::vector<geometry_msgs::msg::Polygon> detection_areas;
 
   lanelet::ConstLineStrings3d bounds;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
+    p.object_check_return_pose_distance =
+      getOrDeclareParameter<double>(*node, ns + "object_check_return_pose_distance");
     p.threshold_distance_object_is_on_center =
       getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
     p.object_check_shiftable_ratio =

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void updateRegisteredRTCStatus(const PathWithLaneId & path)
   {
-    const Point ego_position = planner_data_->self_odometry->pose.pose.position;
+    const auto ego_idx = planner_data_->findEgoIndex(path.points);
 
     for (const auto & left_shift : left_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position);
-      const double finish_distance =
-        calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
+        calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
+      const double finish_distance = start_distance + left_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
@@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & right_shift : right_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position);
-      const double finish_distance =
-        calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
+        calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
+      const double finish_distance = start_distance + right_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
@@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface
     UUID uuid;
     Pose start_pose;
     Pose finish_pose;
+    double relative_longitudinal{0.0};
   };
 
   using RegisteredShiftLineArray = std::vector<RegisteredShiftLine>;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -77,11 +77,6 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width);
 
-double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
-
-bool isCentroidWithinLanelets(
-  const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets);
-
 lanelet::ConstLanelets getAdjacentLane(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
@@ -128,12 +123,7 @@ void compensateDetectionLost(
   ObjectDataArray & other_objects);
 
 void filterTargetObjects(
-  ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
-
-double getRoadShoulderDistance(
-  ObjectData & object, const AvoidancePlanningData & data,
+  ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
@@ -157,9 +147,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   DebugData & debug);
 
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
-  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
-  const double object_check_forward_distance, const bool is_running, DebugData & debug);
+  const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,
+  const std::shared_ptr<const PlannerData> & planner_data, const AvoidancePlanningData & data,
+  const std::shared_ptr<AvoidanceParameters> & parameters,
+  const double object_check_forward_distance, DebugData & debug);
 
 DrivableLanes generateExpandDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray(
     addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9);
   }
 
+  // detection area
+  size_t i = 0;
+  for (const auto & detection_area : debug.detection_areas) {
+    add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
+  }
+
   // misc
   {
     add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
-    add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
     add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
     add(laneletsAsTriangleMarkerArray(
       "drivable_lanes", transformToLanelets(data.drivable_lanes),