Fix the reuse of the previous point for proper stable decisions

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

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -26,6 +26,15 @@
 
 #include <boost/geometry.hpp>
 
+// for writing the svg file
+#include <fstream>
+#include <iostream>
+// for the geometry types
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+// for the svg mapper
+#include <boost/geometry/io/svg/svg_mapper.hpp>
+#include <boost/geometry/io/svg/write.hpp>
+
 namespace drivable_area_expansion
 {
 
@@ -57,6 +66,22 @@ void expandDrivableArea(
 {
   if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return;
 
+  // Declare a stream and an SVG mapper
+  std::ofstream svg1("/home/mclement/Pictures/DA_before.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> before(svg1, 400, 400);
+  // Declare a stream and an SVG mapper
+  std::ofstream svg2("/home/mclement/Pictures/DA_after.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> after(svg2, 400, 400);
+
+  linestring_t left_ls;
+  linestring_t right_ls;
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  before.add(left_ls);
+  before.add(right_ls);
+  before.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  before.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stopwatch;
   const auto & params = planner_data->drivable_area_expansion_parameters;
   const auto & dynamic_objects = *planner_data->dynamic_object;
@@ -126,6 +151,15 @@ void expandDrivableArea(
     std::printf("\tupdate: %2.2fms\n", update_duration);
     std::cout << std::endl;
   }
+
+  left_ls.clear();
+  right_ls.clear();
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  after.add(left_ls);
+  after.add(right_ls);
+  after.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  after.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
 }
 
 point_t convert_point(const Point & p)

planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -46,12 +46,12 @@ bool can_decelerate(
 /// @param [in] ego_data ego data
 /// @param [in] decision the input decision (i.e., which lane to avoid and at what speed)
 /// @param [in] footprint the ego footprint
-/// @param [in] extra_lane_to_avoid an additional lane to avoid. If set, ignore deceleration limits
+/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits
 /// @param [in] params parameters
 /// @return the last pose that is not out of lane (if found)
 std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
   const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
-  const std::optional<lanelet::BasicPolygon2d> & extra_lane_to_avoid, const PlannerParam & params)
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params)
 {
   const auto from_arc_length =
     motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx);
@@ -62,14 +62,16 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
     // TODO(Maxime): binary search
     interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l);
     const auto respect_decel_limit =
-      !params.skip_if_over_max_decel || extra_lane_to_avoid ||
+      !params.skip_if_over_max_decel || prev_slowdown_point ||
       can_decelerate(ego_data, interpolated_point, decision.velocity);
     const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose);
     const auto is_overlap_lane = boost::geometry::overlaps(
       interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon());
     const auto is_overlap_extra_lane =
-      extra_lane_to_avoid &&
-      boost::geometry::overlaps(interpolated_footprint, *extra_lane_to_avoid);
+      prev_slowdown_point &&
+      boost::geometry::overlaps(
+        interpolated_footprint,
+        prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon());
     if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane)
       return interpolated_point;
   }
@@ -79,19 +81,20 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
 /// @brief calculate the slowdown point to insert in the path
 /// @param ego_data ego data (path, velocity, etc)
 /// @param decisions decision (before which point to stop, what lane to avoid entering, etc)
+/// @param prev_slowdown_point previously calculated slowdown point
 /// @param params parameters
 /// @return optional slowdown point to insert in the path
 std::optional<SlowdownToInsert> calculate_slowdown_point(
   const EgoData & ego_data, const std::vector<Slowdown> & decisions,
-  const std::optional<lanelet::BasicPolygon2d> extra_lane_to_avoid, PlannerParam params)
+  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params)
 {
   params.extra_front_offset += params.dist_buffer;
   const auto base_footprint = make_base_footprint(params);
 
   // search for the first slowdown decision for which a stop point can be inserted
   for (const auto & decision : decisions) {
     const auto last_in_lane_pose =
-      calculate_last_in_lane_pose(ego_data, decision, base_footprint, extra_lane_to_avoid, params);
+      calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params);
     if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
   }
   return std::nullopt;

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -129,20 +129,24 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   const auto decisions = calculate_decisions(inputs, params_, logger_);
   const auto calculate_decisions_us = stopwatch.toc("calculate_decisions");
   stopwatch.tic("calc_slowdown_points");
-  if (
-    prev_avoided_lane_ &&
-    (clock_->now() - prev_avoided_lane_start_time_).seconds() > params_.min_decision_duration)
-    prev_avoided_lane_.reset();
-  const auto point_to_insert =
-    calculate_slowdown_point(ego_data, decisions, prev_avoided_lane_, params_);
+  if (  // reset the previous inserted point if the timer expired
+    prev_inserted_point_ &&
+    (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration)
+    prev_inserted_point_.reset();
+  auto point_to_insert =
+    calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_);
   const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points");
   stopwatch.tic("insert_slowdown_points");
   debug_data_.slowdowns.clear();
+  if (  // reset the timer if there is no previous inserted point or if we avoid the same lane
+    point_to_insert &&
+    (!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_;
   if (point_to_insert) {
-    if (!prev_avoided_lane_) {
-      prev_avoided_lane_ = point_to_insert->slowdown.lane_to_avoid.polygon2d().basicPolygon();
-      prev_avoided_lane_start_time_ = clock_->now();
-    }
+    prev_inserted_point_ = point_to_insert;
+    RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
     debug_data_.slowdowns = {*point_to_insert};
     auto path_idx = motion_utils::findNearestSegmentIndex(
                       path->points, point_to_insert->point.point.pose.position) +

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp

@@ -52,8 +52,8 @@ class OutOfLaneModule : public SceneModuleInterface
 
   std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>
     prev_overlapping_path_points_{};
-  std::optional<lanelet::BasicPolygon2d> prev_avoided_lane_{};
-  rclcpp::Time prev_avoided_lane_start_time_{};
+  std::optional<SlowdownToInsert> prev_inserted_point_{};
+  rclcpp::Time prev_inserted_point_time_{};
 
 protected:
   int64_t module_id_{};