feat(avoidance/goal_planner): execute avoidance and pull over simulta…

…neously (autowarefoundation#5979) * feat(avoidance/goal_planner): execute avoidance and pull over simultaneously Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * use utils Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix overlapped Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * reafactor(behavior_path_planner): move isAllowedGoalModification to common Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix readme Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add goal modification condtion to avoidance Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * clean up * revert param Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix param Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * move dead line process Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix condition Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix crop Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix crop * fix typos Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Interplai · May 26, 2024 · a663bf8 · a663bf8
1 parent 56c690d
commit a663bf8
Show file tree

Hide file tree

Showing 16 changed files with 531 additions and 100 deletions.
diff --git a/planning/behavior_path_avoidance_module/README.md b/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           |

diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/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               # [-]

diff --git a/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/...ng/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{};
 };

diff --git a/...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/...ehavior_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 =

diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines)
   AvoidLineArray ret{};
   for (const auto & outline : outlines) {
     ret.push_back(outline.avoid_line);
-    ret.push_back(outline.return_line);
+    if (outline.return_line.has_value()) {
+      ret.push_back(outline.return_line.value());
+    }
 
     std::for_each(
       outline.middle_lines.begin(), outline.middle_lines.end(),
@@ -341,7 +343,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       al_return.object_on_right = utils::avoidance::isOnRight(o);
     }
 
-    if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
+    const bool skip_return_shift = [&]() {
+      if (!utils::isAllowedGoalModification(data_->route_handler)) {
+        return false;
+      }
+      const auto goal_pose = data_->route_handler->getGoalPose();
+      const double goal_longitudinal_distance =
+        motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position);
+      const bool is_return_shift_to_goal =
+        std::abs(al_return.end_longitudinal - goal_longitudinal_distance) <
+        parameters_->object_check_return_pose_distance;
+      if (is_return_shift_to_goal) {
+        return true;
+      }
+      const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position;
+      const bool has_object_near_goal =
+        tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) <
+        parameters_->object_check_goal_distance;
+      return has_object_near_goal;
+    }();
+
+    if (skip_return_shift) {
+      // if the object is close to goal or ego is about to return near GOAL, do not return
+      outlines.emplace_back(al_avoid, std::nullopt);
+    } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       outlines.emplace_back(al_avoid, al_return);
     } else {
       o.reason = "InvalidShiftLine";
@@ -637,35 +662,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess(
     auto & next_outline = outlines.at(i);
 
     const auto & return_line = last_outline.return_line;
-    const auto & avoid_line = next_outline.avoid_line;
+    if (!return_line.has_value()) {
+      ret.push_back(outlines.at(i));
+      break;
+    }
 
-    if (no_conflict(return_line, avoid_line)) {
+    const auto & avoid_line = next_outline.avoid_line;
+    if (no_conflict(return_line.value(), avoid_line)) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
-    const auto merged_shift_line = merge(return_line, avoid_line, generateUUID());
+    const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID());
 
     if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
-    if (same_side_shift(return_line, avoid_line)) {
+    if (same_side_shift(return_line.value(), avoid_line)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
-    if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) {
+    if (
+      within(return_line.value(), avoid_line.end_idx) &&
+      within(avoid_line, return_line->start_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
-    if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) {
+    if (
+      within(return_line.value(), avoid_line.start_idx) &&
+      within(avoid_line, return_line->end_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
@@ -686,7 +719,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess(
 
   for (auto & outline : ret) {
     if (outline.middle_lines.empty()) {
-      const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID());
+      const auto new_line =
+        outline.return_line.has_value()
+          ? fill(outline.avoid_line, outline.return_line.value(), generateUUID())
+          : outline.avoid_line;
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
@@ -701,8 +737,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess(
 
     helper_->alignShiftLinesOrder(outline.middle_lines, false);
 
-    if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) {
-      const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID());
+    if (
+      outline.return_line.has_value() &&
+      outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) {
+      const auto new_line =
+        fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID());
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
@@ -973,6 +1012,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   const bool has_candidate_point = !ret.empty();
   const bool has_registered_point = last_.has_value();
 
+  if (utils::isAllowedGoalModification(data_->route_handler)) {
+    const auto has_object_near_goal =
+      std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
+        return tier4_autoware_utils::calcDistance2d(
+                 data_->route_handler->getGoalPose().position,
+                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+               parameters_->object_check_goal_distance;
+      });
+    if (has_object_near_goal) {
+      RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift");
+      return ret;
+    }
+  }
+
   const auto exist_unavoidable_object = std::any_of(
     data.target_objects.begin(), data.target_objects.end(),
     [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; });
@@ -1027,6 +1080,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     }
   }
 
+  // if last shift line is near the objects, do not add return shift.
+  if (utils::isAllowedGoalModification(data_->route_handler)) {
+    const bool has_last_shift_near_goal =
+      std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
+        return tier4_autoware_utils::calcDistance2d(
+                 last_sl.end.position,
+                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+               parameters_->object_check_goal_distance;
+      });
+    if (has_last_shift_near_goal) {
+      RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects");
+      return ret;
+    }
+  }
+
   // There already is a shift point candidates to go back to center line, but it could be too sharp
   // due to detection noise or timing.
   // Here the return-shift from ego is added for the in case.
@@ -1070,8 +1138,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     return ret;
   }
 
-  const auto remaining_distance = std::min(
-    arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point);
+  const auto remaining_distance =
+    utils::isAllowedGoalModification(data_->route_handler)
+      ? data.to_return_point
+      : std::min(
+          arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point);
 
   // If the avoidance point has already been set, the return shift must be set after the point.
   const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx);

diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -700,11 +700,13 @@ bool isSatisfiedWithCommonCondition(
     return false;
   }
 
-  if (
-    object.longitudinal + object.length / 2 + parameters->object_check_goal_distance >
-    to_goal_distance) {
-    object.reason = "TooNearToGoal";
-    return false;
+  if (!utils::isAllowedGoalModification(planner_data->route_handler)) {
+    if (
+      object.longitudinal + object.length / 2 + parameters->object_check_goal_distance >
+      to_goal_distance) {
+      object.reason = "TooNearToGoal";
+      return false;
+    }
   }
 
   return true;
@@ -1686,7 +1688,9 @@ void fillAdditionalInfoFromLongitudinal(
 {
   for (auto & outline : outlines) {
     fillAdditionalInfoFromLongitudinal(data, outline.avoid_line);
-    fillAdditionalInfoFromLongitudinal(data, outline.return_line);
+    if (outline.return_line.has_value()) {
+      fillAdditionalInfoFromLongitudinal(data, outline.return_line.value());
+    }
 
     std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) {
       fillAdditionalInfoFromLongitudinal(data, line);
@@ -2169,7 +2173,9 @@ double calcDistanceToReturnDeadLine(
   }
 
   // dead line for goal
-  if (parameters->enable_dead_line_for_goal) {
+  if (
+    !utils::isAllowedGoalModification(planner_data->route_handler) &&
+    parameters->enable_dead_line_for_goal) {
     if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
       const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
       const auto to_goal_distance =

diff --git a/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -472,6 +472,10 @@ class GoalPlannerModule : public SceneModuleInterface
   // new turn signal
   TurnSignalInfo calcTurnSignalInfo() const;
 
+  std::optional<BehaviorModuleOutput> last_previous_module_output_{};
+  bool hasPreviousModulePathShapeChanged() const;
+  bool hasDeviatedFromLastPreviousModulePath() const;
+
   // timer for generating pull over path candidates in a separate thread
   void onTimer();
   void onFreespaceParkingTimer();

diff --git a/..._goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/..._goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp
@@ -118,6 +118,11 @@ class PullOverPlannerBase
   }
   virtual ~PullOverPlannerBase() = default;
 
+  void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output)
+  {
+    previous_module_output_ = previous_module_output;
+  }
+
   void setPlannerData(const std::shared_ptr<const PlannerData> planner_data)
   {
     planner_data_ = planner_data;
@@ -131,6 +136,8 @@ class PullOverPlannerBase
   vehicle_info_util::VehicleInfo vehicle_info_;
   LinearRing2d vehicle_footprint_;
   GoalPlannerParameters parameters_;
+
+  BehaviorModuleOutput previous_module_output_;
 };
 }  // namespace behavior_path_planner
 

diff --git a/...or_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/...or_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp
@@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase
 protected:
   PathWithLaneId generateReferencePath(
     const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const;
+  std::optional<PathWithLaneId> cropPrevModulePath(
+    const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const Pose & goal_pose, const double lateral_jerk) const;

diff --git a/...ning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/...ning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp
@@ -49,8 +49,21 @@ PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
 
-bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
-bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);
+double calcLateralDeviationBetweenPaths(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path);
+bool isReferencePath(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path,
+  const double lateral_deviation_thresh);
+
+std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose);
+PathWithLaneId cropForwardPoints(
+  const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length);
+PathWithLaneId extendPath(
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const double extend_length);
+PathWithLaneId extendPath(
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose);
 
 // debug
 MarkerArray createPullOverAreaMarkerArray(