feat(obstacle_cruise)!: type specified stop deccel limit and enabling abandon to stop (#1336)

* feat(obstacle_cruise_planner): add calculation of obstacle distance to ego (autowarefoundation#6057)

Add the arc length from the ego to the obstacle stop point to the stop_reasons topic.

Signed-off-by: Ioannis Souflas <isouflas@gmail.com>

* refactor(obstacle_cruise): refactor a function checkConsistency() (autowarefoundation#7105)

refactor
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* add abandon function

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* fix lib include

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

---------

Signed-off-by: Ioannis Souflas <isouflas@gmail.com>
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
Co-authored-by: Ioannis Souflas <isouflas@gmail.com>

Author: yuki-takagi-66

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -210,3 +210,15 @@
       lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity
       lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path
       lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start
+    stop:
+      type_specified_params:
+        labels: # For the listed types, the node try to read the following type specified values
+          - "default"
+          - "unknown"
+        # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined.
+          # limit_min_acc: common_param.yaml/limit.min_acc
+        unknown:
+          limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred.
+          sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop".
+          sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop".
+          abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit.

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -108,14 +108,15 @@ struct StopObstacle : public TargetObstacleInterface
 {
   StopObstacle(
     const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
-    const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape,
-    const double arg_lon_velocity, const double arg_lat_velocity,
+    const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose,
+    const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity,
     const geometry_msgs::msg::Point arg_collision_point,
     const double arg_dist_to_collide_on_decimated_traj)
   : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
     shape(arg_shape),
     collision_point(arg_collision_point),
-    dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj)
+    dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj),
+    classification(object_classification)
   {
   }
   Shape shape;
@@ -124,6 +125,7 @@ struct StopObstacle : public TargetObstacleInterface
                       // calculateMarginFromObstacleOnCurve() and  should be removed as it can be
                       // replaced by ”dist_to_collide_on_decimated_traj”
   double dist_to_collide_on_decimated_traj;
+  ObjectClassification classification;
 };
 
 struct CruiseObstacle : public TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -263,8 +263,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool is_driving_forward_{true};
   bool enable_slow_down_planning_{false};
 
-  // previous closest obstacle
-  std::shared_ptr<StopObstacle> prev_closest_stop_obstacle_ptr_{nullptr};
+  std::vector<StopObstacle> prev_closest_stop_obstacles_{};
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 };

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -23,6 +23,8 @@
 #include "tier4_autoware_utils/ros/update_param.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
+#include <algorithm>
+#include <limits>
 #include <memory>
 #include <optional>
 #include <string>
@@ -42,7 +44,8 @@ class PlannerInterface
     vehicle_info_(vehicle_info),
     ego_nearest_param_(ego_nearest_param),
     debug_data_ptr_(debug_data_ptr),
-    slow_down_param_(SlowDownParam(node))
+    slow_down_param_(SlowDownParam(node)),
+    stop_param_(StopParam(node, longitudinal_info))
   {
     stop_reasons_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
     velocity_factors_pub_ =
@@ -91,6 +94,7 @@ class PlannerInterface
     updateCommonParam(parameters);
     updateCruiseParam(parameters);
     slow_down_param_.onParam(parameters);
+    stop_param_.onParam(parameters, longitudinal_info_);
   }
 
   Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const
@@ -333,6 +337,84 @@ class PlannerInterface
     double lpf_gain_dist_to_slow_down;
   };
   SlowDownParam slow_down_param_;
+  struct StopParam
+  {
+    struct ObstacleSpecificParams
+    {
+      double limit_min_acc;
+      double sudden_object_acc_threshold;
+      double sudden_object_dist_threshold;
+      bool abandon_to_stop;
+    };
+    const std::unordered_map<uint8_t, std::string> types_maps = {
+      {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"},
+      {ObjectClassification::TRUCK, "truck"},     {ObjectClassification::BUS, "bus"},
+      {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"},
+      {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}};
+    std::unordered_map<std::string, ObstacleSpecificParams> type_specified_param_list;
+    explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info)
+    {
+      const std::string param_prefix = "stop.type_specified_params.";
+      std::vector<std::string> obstacle_labels{"default"};
+      obstacle_labels =
+        node.declare_parameter<std::vector<std::string>>(param_prefix + "labels", obstacle_labels);
+
+      for (const auto & type_str : obstacle_labels) {
+        if (type_str != "default") {
+          ObstacleSpecificParams param{
+            node.declare_parameter<double>(param_prefix + type_str + ".limit_min_acc"),
+            node.declare_parameter<double>(
+              param_prefix + type_str + ".sudden_object_acc_threshold"),
+            node.declare_parameter<double>(
+              param_prefix + type_str + ".sudden_object_dist_threshold"),
+            node.declare_parameter<bool>(param_prefix + type_str + ".abandon_to_stop")};
+
+          param.sudden_object_acc_threshold =
+            std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel);
+          param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold);
+
+          type_specified_param_list.emplace(type_str, param);
+        }
+      }
+    }
+    void onParam(
+      const std::vector<rclcpp::Parameter> & parameters, const LongitudinalInfo & longitudinal_info)
+    {
+      const std::string param_prefix = "stop.type_specified_params.";
+      for (auto & [type_str, param] : type_specified_param_list) {
+        if (type_str == "default") {
+          continue;
+        }
+        tier4_autoware_utils::updateParam<double>(
+          parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc);
+        tier4_autoware_utils::updateParam<double>(
+          parameters, param_prefix + type_str + ".sudden_object_acc_threshold",
+          param.sudden_object_acc_threshold);
+        tier4_autoware_utils::updateParam<double>(
+          parameters, param_prefix + type_str + ".sudden_object_dist_threshold",
+          param.sudden_object_dist_threshold);
+        tier4_autoware_utils::updateParam<bool>(
+          parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop);
+
+        param.sudden_object_acc_threshold =
+          std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel);
+        param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold);
+      }
+    }
+    std::string getParamType(const ObjectClassification label)
+    {
+      const auto type_str = types_maps.at(label.label);
+      if (type_specified_param_list.count(type_str) == 0) {
+        return "default";
+      }
+      return type_str;
+    }
+    ObstacleSpecificParams getParam(const ObjectClassification label)
+    {
+      return type_specified_param_list.at(getParamType(label));
+    }
+  };
+  StopParam stop_param_;
   double moving_object_speed_threshold;
   double moving_object_hysteresis_range;
   std::vector<SlowDownOutput> prev_slow_down_output_;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp

@@ -36,8 +36,7 @@ PoseWithStamp getCurrentObjectPose(
   const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time,
   const rclcpp::Time & current_time, const bool use_prediction);
 
-std::optional<StopObstacle> getClosestStopObstacle(
-  const std::vector<StopObstacle> & stop_obstacles);
+std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle> & stop_obstacles);
 
 template <class T>
 size_t getIndexWithLongitudinalOffset(

planning/obstacle_cruise_planner/src/node.cpp

@@ -1073,9 +1073,9 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
   }
 
   const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
-  return StopObstacle{
-    obstacle.uuid, obstacle.stamp, obstacle.pose,          obstacle.shape,
-    tangent_vel,   normal_vel,     collision_point->first, collision_point->second};
+  return StopObstacle{obstacle.uuid, obstacle.stamp,         obstacle.classification,
+                      obstacle.pose, obstacle.shape,         tangent_vel,
+                      normal_vel,    collision_point->first, collision_point->second};
 }
 
 std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(
@@ -1245,72 +1245,36 @@ void ObstacleCruisePlannerNode::checkConsistency(
   const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
   std::vector<StopObstacle> & stop_obstacles)
 {
-  const auto current_closest_stop_obstacle =
-    obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
-
-  // If previous closest obstacle ptr is not set
-  if (!prev_closest_stop_obstacle_ptr_) {
-    if (current_closest_stop_obstacle) {
-      prev_closest_stop_obstacle_ptr_ =
-        std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
+  for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) {
+    const auto predicted_object_itr = std::find_if(
+      predicted_objects.objects.begin(), predicted_objects.objects.end(),
+      [&prev_closest_stop_obstacle](const PredictedObject & po) {
+        return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid;
+      });
+    // If previous closest obstacle disappear from the perception result, do nothing anymore.
+    if (predicted_object_itr == predicted_objects.objects.end()) {
+      continue;
     }
-    return;
-  }
-
-  // Put previous closest target obstacle if necessary
-  const auto predicted_object_itr = std::find_if(
-    predicted_objects.objects.begin(), predicted_objects.objects.end(),
-    [&](PredictedObject predicted_object) {
-      return tier4_autoware_utils::toHexString(predicted_object.object_id) ==
-             prev_closest_stop_obstacle_ptr_->uuid;
-    });
-
-  // If previous closest obstacle is not in the current perception lists
-  // just return the current target obstacles
-  if (predicted_object_itr == predicted_objects.objects.end()) {
-    return;
-  }
 
-  // Previous closest obstacle is in the perception lists
-  const auto obstacle_itr = std::find_if(
-    stop_obstacles.begin(), stop_obstacles.end(),
-    [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; });
-
-  // Previous closest obstacle is both in the perception lists and target obstacles
-  if (obstacle_itr != stop_obstacles.end()) {
-    if (current_closest_stop_obstacle) {
-      if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) {
-        // prev_closest_obstacle is current_closest_stop_obstacle just return the target
-        // obstacles(in target obstacles)
-        prev_closest_stop_obstacle_ptr_ =
-          std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
-      } else {
-        // New obstacle becomes new stop obstacle
-        prev_closest_stop_obstacle_ptr_ =
-          std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
+    const auto is_disappeared_from_stop_obstacle = std::none_of(
+      stop_obstacles.begin(), stop_obstacles.end(),
+      [&prev_closest_stop_obstacle](const StopObstacle & so) {
+        return so.uuid == prev_closest_stop_obstacle.uuid;
+      });
+    if (is_disappeared_from_stop_obstacle) {
+      // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop"
+      // condition is satisfied
+      const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds();
+      if (
+        predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
+          behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
+        elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
+        stop_obstacles.push_back(prev_closest_stop_obstacle);
       }
-    } else {
-      // Previous closest stop obstacle becomes cruise obstacle
-      prev_closest_stop_obstacle_ptr_ = nullptr;
-    }
-  } else {
-    // prev obstacle is not in the target obstacles, but in the perception list
-    const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds();
-    if (
-      predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
-        behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
-      elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
-      stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_);
-      return;
-    }
-
-    if (current_closest_stop_obstacle) {
-      prev_closest_stop_obstacle_ptr_ =
-        std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
-    } else {
-      prev_closest_stop_obstacle_ptr_ = nullptr;
     }
   }
+
+  prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles);
 }
 
 bool ObstacleCruisePlannerNode::isObstacleCrossing(