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

…neously (#5979) * feat(avoidance/goal_planner): execute avoidance and pull over simultaneously Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]> * use utils Signed-off-by: kosuke55 <[email protected]> * fix overlapped Signed-off-by: kosuke55 <[email protected]> * reafactor(behavior_path_planner): move isAllowedGoalModification to common Signed-off-by: kosuke55 <[email protected]> * fix readme Signed-off-by: kosuke55 <[email protected]> * add goal modification condtion to avoidance Signed-off-by: kosuke55 <[email protected]> * clean up * revert param Signed-off-by: kosuke55 <[email protected]> * fix param Signed-off-by: kosuke55 <[email protected]> * move dead line process Signed-off-by: kosuke55 <[email protected]> * fix condition Signed-off-by: kosuke55 <[email protected]> * fix crop Signed-off-by: kosuke55 <[email protected]> * fix crop * fix typos Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jan 16, 2024 · c3f8e0a · c3f8e0a
1 parent a3f90f0
commit c3f8e0a
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(