feat(out_of_lane): improve reuse of previous decision (#1017)

* Do not directly reuse a prev stop point but project it on the new path

Signed-off-by: Maxime CLEMENT <[email protected]>

* Improve reuse of the previously inserted stop point

Signed-off-by: Maxime CLEMENT <[email protected]>

* Fix precision of inserted stop point

Signed-off-by: Maxime CLEMENT <[email protected]>

---------

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp

@@ -49,7 +49,8 @@ OverlapRanges calculate_overlapping_ranges(
 /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path
 /// @param [in] lanelets lanelets used to calculate the overlaps
 /// @param [in] params parameters
-/// @return the overlapping ranges found between the footprints and the lanelets
+/// @return the overlapping ranges found between the footprints and the lanelets, sorted by
+/// increasing arc length along the path
 OverlapRanges calculate_overlapping_ranges(
   const std::vector<lanelet::BasicPolygon2d> & path_footprints,
   const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets,

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -62,18 +62,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   ego_data.path.points = path->points;
   ego_data.first_path_idx =
     motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
-  for (const auto & p : prev_overlapping_path_points_) {
-    const auto nearest_idx =
-      motion_utils::findNearestIndex(ego_data.path.points, p.point.pose, 1.0);
-    const auto insert_idx =
-      motion_utils::findNearestSegmentIndex(ego_data.path.points, p.point.pose, 1.0);
-    if (nearest_idx && insert_idx && *insert_idx > ego_data.first_path_idx) {
-      if (
-        tier4_autoware_utils::calcDistance2d(
-          ego_data.path.points[*nearest_idx].point.pose, p.point.pose) > 0.5)
-        ego_data.path.points.insert(std::next(ego_data.path.points.begin(), *insert_idx), p);
-    }
-  }
   motion_utils::removeOverlapPoints(ego_data.path.points);
   ego_data.velocity = planner_data_->current_velocity->twist.linear.x;
   ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold;
@@ -113,10 +101,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   stopwatch.tic("calculate_overlapping_ranges");
   const auto ranges =
     calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_);
-  prev_overlapping_path_points_.clear();
-  for (const auto & range : ranges)
-    prev_overlapping_path_points_.push_back(
-      ego_data.path.points[ego_data.first_path_idx + range.entering_path_idx]);
   const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges");
   // Calculate stop and slowdown points
   stopwatch.tic("calculate_decisions");
@@ -143,7 +127,28 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() ==
                                 point_to_insert->slowdown.lane_to_avoid.id()))
     prev_inserted_point_time_ = clock_->now();
-  if (!point_to_insert && prev_inserted_point_) point_to_insert = prev_inserted_point_;
+  // reuse previous stop point if there is no new one or if its velocity is not higher than the new
+  // one and its arc length is lower
+  const auto should_use_prev_inserted_point = [&]() {
+    if (
+      point_to_insert && prev_inserted_point_ &&
+      prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) {
+      const auto arc_length = motion_utils::calcSignedArcLength(
+        path->points, 0LU, point_to_insert->point.point.pose.position);
+      const auto prev_arc_length = motion_utils::calcSignedArcLength(
+        path->points, 0LU, prev_inserted_point_->point.point.pose.position);
+      return prev_arc_length < arc_length;
+    }
+    return !point_to_insert && prev_inserted_point_;
+  }();
+  if (should_use_prev_inserted_point) {
+    // if the path changed the prev point is no longer on the path so we project it
+    const auto insert_arc_length = motion_utils::calcSignedArcLength(
+      path->points, 0LU, prev_inserted_point_->point.point.pose.position);
+    prev_inserted_point_->point.point.pose =
+      motion_utils::calcInterpolatedPose(path->points, insert_arc_length);
+    point_to_insert = prev_inserted_point_;
+  }
   if (point_to_insert) {
     prev_inserted_point_ = point_to_insert;
     RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
@@ -152,8 +157,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
                       path->points, point_to_insert->point.point.pose.position) +
                     1;
     planning_utils::insertVelocity(
-      *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx,
-      params_.precision);
+      *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx);
     if (point_to_insert->slowdown.velocity == 0.0) {
       tier4_planning_msgs::msg::StopFactor stop_factor;
       stop_factor.stop_pose = point_to_insert->point.point.pose;

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp

@@ -50,8 +50,6 @@ class OutOfLaneModule : public SceneModuleInterface
 private:
   PlannerParam params_;
 
-  std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>
-    prev_overlapping_path_points_{};
   std::optional<SlowdownToInsert> prev_inserted_point_{};
   rclcpp::Time prev_inserted_point_time_{};