feat(obstacle_cruise_planner)!: improve stop and cruise behavior for …

…cut-in & out (autowarefoundation#8072) (#1585)

feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out (autowarefoundation#8072)
--------
Signed-off-by: Berkay Karaman <[email protected]>
Co-authored-by: Berkay Karaman <[email protected]>

planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -29,15 +29,26 @@
       suppress_sudden_obstacle_stop: false
 
       stop_obstacle_type:
-        unknown: true
-        car: true
-        truck: true
-        bus: true
-        trailer: true
-        motorcycle: true
-        bicycle: true
-        pedestrian: true
         pointcloud: false
+        inside:
+          unknown: true
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: true
+
+        outside:
+          unknown: false
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: true
 
       cruise_obstacle_type:
         inside:
@@ -99,6 +110,12 @@
       stop:
         max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint
         max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint
+        min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s]
+        outside_obstacle:
+          max_lateral_time_margin: 4.0 # time threshold of lateral margin between the obstacles and ego's footprint [s]
+          num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego
+          pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss]
+          bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss]
         crossing_obstacle:
           collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
@@ -108,6 +125,8 @@
           obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
           ego_obstacle_overlap_time_threshold : 2.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
           max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
+          max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
+          num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego
         yield:
           enable_yield: true
           lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

@@ -58,10 +58,13 @@ struct Obstacle
   Obstacle(
     const rclcpp::Time & arg_stamp, const PredictedObject & object,
     const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance,
-    const double lat_dist_from_obstacle_to_traj)
+    const double lat_dist_from_obstacle_to_traj, const double longitudinal_velocity,
+    const double approach_velocity)
   : stamp(arg_stamp),
     ego_to_obstacle_distance(ego_to_obstacle_distance),
     lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
+    longitudinal_velocity(longitudinal_velocity),
+    approach_velocity(approach_velocity),
     pose(arg_pose),
     orientation_reliable(true),
     twist(object.kinematics.initial_twist_with_covariance.twist),
@@ -94,6 +97,8 @@ struct Obstacle
   rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
   double ego_to_obstacle_distance;
   double lat_dist_from_obstacle_to_traj;
+  double longitudinal_velocity;
+  double approach_velocity;
 
   // for PredictedObject
   geometry_msgs::msg::Pose pose;  // interpolated with the current stamp

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp

@@ -37,6 +37,7 @@
 #include <string>
 #include <tuple>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace autoware::motion_planning
@@ -79,24 +80,29 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
     const double precise_lateral_dist) const;
+  std::optional<std::pair<geometry_msgs::msg::Point, double>>
+  createCollisionPointForOutsideStopObstacle(
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+    const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+    const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const;
   std::optional<StopObstacle> createStopObstacleForPointCloud(
     const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
     const double precise_lateral_dist) const;
+  bool isInsideStopObstacle(const uint8_t label) const;
+  bool isOutsideStopObstacle(const uint8_t label) const;
   bool isStopObstacle(const uint8_t label) const;
   bool isInsideCruiseObstacle(const uint8_t label) const;
   bool isOutsideCruiseObstacle(const uint8_t label) const;
   bool isCruiseObstacle(const uint8_t label) const;
   bool isSlowDownObstacle(const uint8_t label) const;
-  std::optional<geometry_msgs::msg::Point> createCollisionPointForStopObstacle(
-    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-    const Obstacle & obstacle) const;
   std::optional<CruiseObstacle> createYieldCruiseObstacle(
     const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points);
   std::optional<std::vector<CruiseObstacle>> findYieldCruiseObstacles(
     const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points);
   std::optional<CruiseObstacle> createCruiseObstacle(
-    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-    const Obstacle & obstacle, const double precise_lat_dist);
+    const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
+    const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
+    const double precise_lat_dist);
   std::optional<std::vector<PointWithStamp>> createCollisionPointsForInsideCruiseObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
     const Obstacle & obstacle) const;
@@ -129,7 +135,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool isFrontCollideObstacle(
     const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
     const size_t first_collision_idx) const;
-
+  double calcTimeToReachCollisionPoint(
+    const Odometry & odometry, const geometry_msgs::msg::Point & collision_point,
+    const std::vector<TrajectoryPoint> & traj_points, const double abs_ego_offset) const;
   bool enable_debug_info_;
   bool enable_calculation_time_info_;
   bool use_pointcloud_for_stop_;
@@ -140,7 +148,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   double min_safe_distance_margin_on_curve_;
   bool suppress_sudden_obstacle_stop_;
 
-  std::vector<int> stop_obstacle_types_;
+  std::vector<int> inside_stop_obstacle_types_;
+  std::vector<int> outside_stop_obstacle_types_;
   std::vector<int> inside_cruise_obstacle_types_;
   std::vector<int> outside_cruise_obstacle_types_;
   std::vector<int> slow_down_obstacle_types_;
@@ -226,11 +235,20 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double ego_obstacle_overlap_time_threshold;
     double max_prediction_time_for_collision_check;
     double crossing_obstacle_traj_angle_threshold;
+    int num_of_predicted_paths_for_outside_cruise_obstacle;
+    int num_of_predicted_paths_for_outside_stop_obstacle;
+    double pedestrian_deceleration_rate;
+    double bicycle_deceleration_rate;
     // obstacle hold
     double stop_obstacle_hold_time_threshold;
+    // reach collision point
+    double min_velocity_to_reach_collision_point;
     // prediction resampling
     double prediction_resampling_time_interval;
     double prediction_resampling_time_horizon;
+    // max lateral time margin
+    double max_lat_time_margin_for_stop;
+    double max_lat_time_margin_for_cruise;
     // max lateral margin
     double max_lat_margin_for_stop;
     double max_lat_margin_for_stop_against_unknown;

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp

@@ -43,6 +43,11 @@ std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const Obstacle & obstacle, const bool is_driving_forward,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
 
+std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
+  const std::vector<TrajectoryPoint> & traj_points, const size_t collision_idx,
+  const std::vector<PointWithStamp> & collision_points, const bool is_driving_forward,
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
+
 std::vector<PointWithStamp> getCollisionPoints(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape,