feat(run_out): maintain run out stop wall (#6732)

* feat(run_out): maintain run_out stop walls for a given time Signed-off-by: Daniel Sanchez <[email protected]> * remove printouts Signed-off-by: Daniel Sanchez <[email protected]> * space Signed-off-by: Daniel Sanchez <[email protected]> * add stop wall debug markers when maintaining stop points Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Apr 5, 2024 · 1eef4c0 · 1eef4c0
1 parent 8046005
commit 1eef4c0
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 12 deletions.
diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
@@ -17,6 +17,7 @@
       ego_cut_line_length: 3.0    # The width of the ego's cut line
       ego_footprint_extra_margin: 0.5 # [m] expand the ego vehicles' footprint by this value on all sides when building the ego footprint path
       keep_obstacle_on_path_time_threshold: 1.0 # [s] How much time a previous run out target obstacle is kept in the run out candidate list if it enters the ego path.
+      keep_stop_point_time: 1.0   # [s] If a stop point is issued by this module, keep the stop point for this many seconds. Only works if approach is disabled
 
       # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:

diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp
@@ -77,6 +77,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
       getOrDeclareParameter<double>(node, ns + ".ego_footprint_extra_margin");
     p.keep_obstacle_on_path_time_threshold =
       getOrDeclareParameter<double>(node, ns + ".keep_obstacle_on_path_time_threshold");
+    p.keep_stop_point_time = getOrDeclareParameter<double>(node, ns + ".keep_stop_point_time");
   }
 
   {

diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp
@@ -144,6 +144,16 @@ bool RunOutModule::modifyPathVelocity(
   debug_ptr_->setDebugValues(
     DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0);
 
+  // If no new obstacle is detected, we check if the stop point should be maintained for more time.
+  auto should_maintain_stop_point = [&](const bool is_stopping_point_inserted) -> bool {
+    if (!stop_point_generation_time_.has_value() || is_stopping_point_inserted) {
+      return false;
+    }
+    const auto now = clock_->now().seconds();
+    const double elapsed_time_since_stop_wall_was_generated =
+      (now - stop_point_generation_time_.value());
+    return elapsed_time_since_stop_wall_was_generated < planner_param_.run_out.keep_stop_point_time;
+  };
   // insert stop point for the detected obstacle
   if (planner_param_.approaching.enable) {
     // after a certain amount of time has elapsed since the ego stopped,
@@ -152,7 +162,23 @@ bool RunOutModule::modifyPathVelocity(
       dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path);
   } else {
     // just insert zero velocity for stopping
-    insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path);
+    std::optional<geometry_msgs::msg::Pose> stop_point;
+    const bool is_stopping_point_inserted = insertStoppingVelocity(
+      dynamic_obstacle, current_pose, current_vel, current_acc, *path, stop_point);
+
+    stop_point_generation_time_ = (is_stopping_point_inserted)
+                                    ? std::make_optional(clock_->now().seconds())
+                                    : stop_point_generation_time_;
+    last_stop_point_ = (is_stopping_point_inserted) ? stop_point : last_stop_point_;
+
+    const bool is_maintain_stop_point = should_maintain_stop_point(is_stopping_point_inserted);
+    if (is_maintain_stop_point) {
+      insertStopPoint(last_stop_point_, *path);
+      // debug
+      debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
+      debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose(
+        *last_stop_point_, planner_param_.vehicle_param.base_to_front, 0, 0));
+    }
   }
 
   // apply max jerk limit if the ego can't stop with specified max jerk and acc
@@ -230,10 +256,8 @@ std::optional<DynamicObstacle> RunOutModule::detectCollision(
 
     debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points);
     debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point);
-
     return obstacle_selected;
   }
-
   // no collision detected
   first_detected_time_.reset();
   return {};
@@ -717,14 +741,14 @@ std::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
   return stop_point;
 }
 
-void RunOutModule::insertStopPoint(
+bool RunOutModule::insertStopPoint(
   const std::optional<geometry_msgs::msg::Pose> stop_point,
   autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   // no stop point
   if (!stop_point) {
     RCLCPP_DEBUG_STREAM(logger_, "already has same point");
-    return;
+    return false;
   }
 
   // find nearest point index behind the stop point
@@ -736,15 +760,15 @@ void RunOutModule::insertStopPoint(
   if (
     insert_idx == path.points.size() - 1 &&
     planning_utils::isAheadOf(*stop_point, path.points.at(insert_idx).point.pose)) {
-    return;
+    return false;
   }
 
   // to PathPointWithLaneId
   autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id;
   stop_point_with_lane_id = path.points.at(nearest_seg_idx);
   stop_point_with_lane_id.point.pose = *stop_point;
-
   planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx);
+  return true;
 }
 
 void RunOutModule::insertVelocityForState(
@@ -806,14 +830,24 @@ void RunOutModule::insertVelocityForState(
   }
 }
 
-void RunOutModule::insertStoppingVelocity(
+bool RunOutModule::insertStoppingVelocity(
   const std::optional<DynamicObstacle> & dynamic_obstacle,
   const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
   PathWithLaneId & output_path)
 {
-  const auto stop_point =
+  std::optional<geometry_msgs::msg::Pose> stopping_point;
+  return insertStoppingVelocity(
+    dynamic_obstacle, current_pose, current_vel, current_acc, output_path, stopping_point);
+}
+
+bool RunOutModule::insertStoppingVelocity(
+  const std::optional<DynamicObstacle> & dynamic_obstacle,
+  const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
+  PathWithLaneId & output_path, std::optional<geometry_msgs::msg::Pose> & stopping_point)
+{
+  stopping_point =
     calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc);
-  insertStopPoint(stop_point, output_path);
+  return insertStopPoint(stopping_point, output_path);
 }
 
 void RunOutModule::insertApproachingVelocity(

diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp
@@ -66,6 +66,9 @@ class RunOutModule : public SceneModuleInterface
   std::shared_ptr<RunOutDebug> debug_ptr_;
   std::unique_ptr<run_out_utils::StateMachine> state_machine_;
   std::shared_ptr<rclcpp::Time> first_detected_time_;
+  std::optional<double> stop_point_generation_time_;
+  std::optional<geometry_msgs::msg::Pose> last_stop_point_;
+
   std::optional<unique_identifier_msgs::msg::UUID> last_stop_obstacle_uuid_;
   std::unordered_map<std::string, double> obstacle_in_ego_path_times_;
 
@@ -119,7 +122,7 @@ class RunOutModule : public SceneModuleInterface
     const geometry_msgs::msg::Pose & current_pose, const float current_vel,
     const float current_acc) const;
 
-  void insertStopPoint(
+  bool insertStopPoint(
     const std::optional<geometry_msgs::msg::Pose> stop_point,
     autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 
@@ -128,11 +131,16 @@ class RunOutModule : public SceneModuleInterface
     const PlannerParam & planner_param, const PathWithLaneId & smoothed_path,
     PathWithLaneId & output_path);
 
-  void insertStoppingVelocity(
+  bool insertStoppingVelocity(
     const std::optional<DynamicObstacle> & dynamic_obstacle,
     const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
     PathWithLaneId & output_path);
 
+  bool insertStoppingVelocity(
+    const std::optional<DynamicObstacle> & dynamic_obstacle,
+    const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
+    PathWithLaneId & output_path, std::optional<geometry_msgs::msg::Pose> & stopping_point);
+
   void insertApproachingVelocity(
     const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose,
     const float approaching_vel, const float approach_margin, PathWithLaneId & output_path);

diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp
@@ -68,6 +68,7 @@ struct RunOutParam
   double ego_cut_line_length;
   double ego_footprint_extra_margin;
   double keep_obstacle_on_path_time_threshold;
+  double keep_stop_point_time;
   float detection_distance;
   float detection_span;
   float min_vel_ego_kmph;