fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

Diff for: planning/autoware_obstacle_cruise_planner/README.md

@@ -77,26 +77,14 @@ start
 
 partition filter_stop_obstacle_for_predicted_object {
 :is_stop_obstacle_type;
-note right
- - reference pose
- - reference path:
-      generated by spline interpolation of centerline path
-      with dense interval
- - current lanelets
- - drivable bounds
- - avoidance start point
-      calculate the point where the ego should start avoidance maneuver
-      depending on traffic signal.
- - avoidance return point
-      calculate the point where the ego should return original lane
-      depending on traffic signal and goal position.
-end note
 
 partition filter_inside_stop_obstacle_for_predicted_object {
 :filter by lateral distance;
 
 :filter by label;
 
+:is_stop_obstacle_velocity;
+
 :check if the obstacle really collides with the trajectory;
 
 :is_crossing_transient_obstacle;
@@ -106,17 +94,28 @@ partition filter_outside_stop_obstacle_for_predicted_object {
 }
 
 :checkConsistency;
+note right
+consistent
+end note
 }
 
 partition filter_stop_obstacle_for_point_cloud {
 }
 
 :concat stop obstacles;
+note right
+concat stop obstacles by predicted objects and point cloud
+end note
 
 partition generate_stop_trajectory {
 :calculate dist to collide;
 
 :calc_desired_stop_margin;
+note right
+- whether the ego is cloes to the goal
+- whether the ego is on the curve
+- whether there is stop point by behavior's stop point
+end note
 
 :calc_candidate_zero_vel_dist;
 

Diff for: planning/autoware_obstacle_cruise_planner/config/stop.param.yaml

@@ -18,29 +18,29 @@
         min_safe_distance_margin: 3.0 # [m]
       suppress_sudden_obstacle_stop: false
 
-      obstacle_type:
-        pointcloud: false
-        inside:
-          unknown: true
-          car: true
-          truck: true
-          bus: true
-          trailer: true
-          motorcycle: true
-          bicycle: true
-          pedestrian: true
+      obstacle_filtering:
+        obstacle_type:
+          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
+          outside:
+            unknown: false
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: true
+            pedestrian: true
 
-      behavior_determination:
         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]
@@ -55,8 +55,8 @@
         stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
 
         # hysteresis for cruise and stop
-        obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
-        obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+        obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+        obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
 
       type_specified_params:
         labels: # For the listed types, the node try to read the following type specified values

Diff for: planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop/obstacle_stop_module.hpp

@@ -139,11 +139,12 @@ class ObstacleStopModule
     ignore_crossing_obstacle_ =
       getOrDeclareParameter<bool>(node, "stop.common.ignore_crossing_obstacle");
 
-    inside_stop_obstacle_types_ =
-      obstacle_cruise_utils::getTargetObjectType(node, "stop.obstacle_type.inside.");
-    outside_stop_obstacle_types_ =
-      obstacle_cruise_utils::getTargetObjectType(node, "stop.obstacle_type.outside.");
-    use_pointcloud_for_stop_ = getOrDeclareParameter<bool>(node, "stop.obstacle_type.pointcloud");
+    inside_stop_obstacle_types_ = obstacle_cruise_utils::getTargetObjectType(
+      node, "stop.obstacle_filtering.obstacle_type.inside.");
+    outside_stop_obstacle_types_ = obstacle_cruise_utils::getTargetObjectType(
+      node, "stop.obstacle_filtering.obstacle_type.outside.");
+    use_pointcloud_for_stop_ =
+      getOrDeclareParameter<bool>(node, "stop.obstacle_filtering.obstacle_type.pointcloud");
 
     velocity_factors_pub_ = node.create_publisher<VelocityFactorArray>(
       "/planning/velocity_factors/obstacle_cruise/stop", 1);
@@ -696,6 +697,10 @@ class ObstacleStopModule
       return std::nullopt;
     }
 
+    if (!is_stop_obstacle_velocity(obstacle)) {
+      return std::nullopt;
+    }
+
     // calculate collision points with trajectory with lateral stop margin
     const auto traj_polys_with_lat_margin = obstacle_cruise_utils::createOneStepPolygons(
       traj_points, vehicle_info, odometry.pose.pose, max_lat_margin_for_stop,
@@ -722,6 +727,25 @@ class ObstacleStopModule
                         collision_point->first,  collision_point->second};
   }
 
+  bool is_stop_obstacle_velocity(const Obstacle & obstacle) const
+  {
+    const bool is_prev_obstacle_stop =
+      obstacle_cruise_utils::getObstacleFromUuid(prev_stop_object_obstacles_, obstacle.uuid)
+        .has_value();
+
+    if (is_prev_obstacle_stop) {
+      if (stop_param_.obstacle_velocity_threshold_from_stop < obstacle.longitudinal_velocity) {
+        return false;
+      }
+      return true;
+    }
+
+    if (obstacle.longitudinal_velocity < stop_param_.obstacle_velocity_threshold_to_stop) {
+      return true;
+    }
+    return false;
+  }
+
   bool is_crossing_transient_obstacle(
     const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
     const Obstacle & obstacle, const double dist_to_bumper,
@@ -812,7 +836,7 @@ class ObstacleStopModule
         const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds();
         if (
           predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
-            stop_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
+            stop_param_.obstacle_velocity_threshold_from_stop &&
           elapsed_time < stop_param_.stop_obstacle_hold_time_threshold) {
           stop_obstacles.push_back(prev_closest_stop_obstacle);
         }

Diff for: planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop/types.hpp

@@ -144,8 +144,8 @@ struct StopParam
   double pedestrian_deceleration_rate;
   double bicycle_deceleration_rate;
   double stop_obstacle_hold_time_threshold;
-  double obstacle_velocity_threshold_from_cruise_to_stop;
-  double obstacle_velocity_threshold_from_stop_to_cruise;
+  double obstacle_velocity_threshold_to_stop;
+  double obstacle_velocity_threshold_from_stop;
 
   struct ObstacleSpecificParams
   {
@@ -170,27 +170,27 @@ struct StopParam
 
     // behavior determination
     collision_time_margin = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.crossing_obstacle.collision_time_margin");
+      node, "stop.obstacle_filtering.crossing_obstacle.collision_time_margin");
     max_lat_margin_for_stop =
-      getOrDeclareParameter<double>(node, "stop.behavior_determination.max_lat_margin");
-    max_lat_margin_for_stop_against_unknown = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.max_lat_margin_against_unknown");
+      getOrDeclareParameter<double>(node, "stop.obstacle_filtering.max_lat_margin");
+    max_lat_margin_for_stop_against_unknown =
+      getOrDeclareParameter<double>(node, "stop.obstacle_filtering.max_lat_margin_against_unknown");
     min_velocity_to_reach_collision_point = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.min_velocity_to_reach_collision_point");
+      node, "stop.obstacle_filtering.min_velocity_to_reach_collision_point");
     max_lat_time_margin_for_stop = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.outside_obstacle.max_lateral_time_margin");
+      node, "stop.obstacle_filtering.outside_obstacle.max_lateral_time_margin");
     num_of_predicted_paths_for_outside_stop_obstacle = getOrDeclareParameter<int>(
-      node, "stop.behavior_determination.outside_obstacle.num_of_predicted_paths");
+      node, "stop.obstacle_filtering.outside_obstacle.num_of_predicted_paths");
     pedestrian_deceleration_rate = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.outside_obstacle.pedestrian_deceleration_rate");
+      node, "stop.obstacle_filtering.outside_obstacle.pedestrian_deceleration_rate");
     bicycle_deceleration_rate = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.outside_obstacle.bicycle_deceleration_rate");
+      node, "stop.obstacle_filtering.outside_obstacle.bicycle_deceleration_rate");
     stop_obstacle_hold_time_threshold = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.stop_obstacle_hold_time_threshold");
-    obstacle_velocity_threshold_from_cruise_to_stop = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop");
-    obstacle_velocity_threshold_from_stop_to_cruise = getOrDeclareParameter<double>(
-      node, "stop.behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise");
+      node, "stop.obstacle_filtering.stop_obstacle_hold_time_threshold");
+    obstacle_velocity_threshold_to_stop = getOrDeclareParameter<double>(
+      node, "stop.obstacle_filtering.obstacle_velocity_threshold_to_stop");
+    obstacle_velocity_threshold_from_stop = getOrDeclareParameter<double>(
+      node, "stop.obstacle_filtering.obstacle_velocity_threshold_from_stop");
 
     const std::string param_prefix = "stop.type_specified_params.";
     std::vector<std::string> obstacle_labels{"default"};
@@ -226,30 +226,30 @@ struct StopParam
       parameters, "stop.hold_stop_distance_threshold", hold_stop_distance_threshold);
 
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.crossing_obstacle.collision_time_margin",
+      parameters, "stop.obstacle_filtering.crossing_obstacle.collision_time_margin",
       collision_time_margin);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.max_lat_margin", max_lat_margin_for_stop);
+      parameters, "stop.obstacle_filtering.max_lat_margin", max_lat_margin_for_stop);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.max_lat_margin_against_unknown",
+      parameters, "stop.obstacle_filtering.max_lat_margin_against_unknown",
       max_lat_margin_for_stop_against_unknown);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.min_velocity_to_reach_collision_point",
+      parameters, "stop.obstacle_filtering.min_velocity_to_reach_collision_point",
       min_velocity_to_reach_collision_point);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.outside_obstacle.max_lateral_time_margin",
+      parameters, "stop.obstacle_filtering.outside_obstacle.max_lateral_time_margin",
       max_lat_time_margin_for_stop);
     autoware::universe_utils::updateParam<int>(
-      parameters, "stop.behavior_determination.outside_obstacle.num_of_predicted_paths",
+      parameters, "stop.obstacle_filtering.outside_obstacle.num_of_predicted_paths",
       num_of_predicted_paths_for_outside_stop_obstacle);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.outside_obstacle.pedestrian_deceleration_rate",
+      parameters, "stop.obstacle_filtering.outside_obstacle.pedestrian_deceleration_rate",
       pedestrian_deceleration_rate);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination.outside_obstacle.bicycle_deceleration_rate",
+      parameters, "stop.obstacle_filtering.outside_obstacle.bicycle_deceleration_rate",
       bicycle_deceleration_rate);
     autoware::universe_utils::updateParam<double>(
-      parameters, "stop.behavior_determination_obstacle_hold_time_threshold",
+      parameters, "stop.obstacle_filtering_obstacle_hold_time_threshold",
       stop_obstacle_hold_time_threshold);
 
     const std::string param_prefix = "stop.type_specified_params.";