feat: add predefined deceleration rate for VRUs

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -114,6 +114,8 @@
         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]
 

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

@@ -237,6 +237,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     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

planning/autoware_obstacle_cruise_planner/src/node.cpp

@@ -368,6 +368,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths");
   num_of_predicted_paths_for_outside_stop_obstacle = node.declare_parameter<int>(
     "behavior_determination.stop.outside_obstacle.num_of_predicted_paths");
+  pedestrian_deceleration_rate = node.declare_parameter<double>(
+    "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate");
+  bicycle_deceleration_rate = node.declare_parameter<double>(
+    "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate");
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -475,6 +479,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
   autoware::universe_utils::updateParam<int>(
     parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths",
     num_of_predicted_paths_for_outside_stop_obstacle);
+  autoware::universe_utils::updateParam<double>(
+    parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate",
+    pedestrian_deceleration_rate);
+  autoware::universe_utils::updateParam<double>(
+    parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate",
+    bicycle_deceleration_rate);
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -1664,10 +1674,23 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacleForPred
       return std::nullopt;
     }
 
+    // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking
+    // they will stop with a predefined deceleration rate to avoid unnecessary stops.
+    double resample_time_horizon = p.prediction_resampling_time_horizon;
+    if (obstacle.classification.label == ObjectClassification::PEDESTRIAN) {
+      resample_time_horizon =
+        std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) /
+        p.pedestrian_deceleration_rate;
+    } else if (obstacle.classification.label == ObjectClassification::BICYCLE) {
+      resample_time_horizon =
+        std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) /
+        p.bicycle_deceleration_rate;
+    }
+
     // Get the highest confidence predicted path
     const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths(
-      obstacle.predicted_paths, p.prediction_resampling_time_interval,
-      p.prediction_resampling_time_horizon, p.num_of_predicted_paths_for_outside_stop_obstacle);
+      obstacle.predicted_paths, p.prediction_resampling_time_interval, resample_time_horizon,
+      p.num_of_predicted_paths_for_outside_stop_obstacle);
     if (resampled_predicted_paths.empty()) {
       return std::nullopt;
     }