feat(obstacle_cruise_planner): stabler slow down

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -86,15 +86,15 @@
           collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
       cruise:
-        max_lat_margin: 0.8 # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
         outside_obstacle:
           obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
           ego_obstacle_overlap_time_threshold : 1.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
 
       slow_down:
-        max_lat_margin: 0.7  # lateral margin between obstacle and trajectory band with ego's width
-        lat_hysteresis_margin: 0.1
+        max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
+        lat_hysteresis_margin: 0.2
 
     cruise:
       pid_based_planner:
@@ -168,3 +168,7 @@
       max_ego_velocity: 8.0
 
       time_margin_on_target_velocity: 1.5 # [s]
+
+      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

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -24,6 +24,9 @@
 
 #include <memory>
 #include <optional>
+#include <string>
+#include <tuple>
+#include <utility>
 #include <vector>
 
 class PlannerInterface
@@ -161,10 +164,35 @@ class PlannerInterface
   }
 
 private:
-  double calculateSlowDownVelocity(const SlowDownObstacle & obstacle) const;
-  double calculateDistanceToSlowDownWithConstraints(
+  struct SlowDownOutput
+  {
+    SlowDownOutput() = default;
+    SlowDownOutput(
+      const std::string & arg_uuid, const std::vector<TrajectoryPoint> & traj_points,
+      const std::optional<size_t> & start_idx, const std::optional<size_t> & end_idx,
+      const double arg_target_vel, const double arg_precise_lat_dist)
+    : uuid(arg_uuid), target_vel(arg_target_vel), precise_lat_dist(arg_precise_lat_dist)
+    {
+      if (start_idx) {
+        start_point = traj_points.at(*start_idx).pose;
+      }
+      if (end_idx) {
+        end_point = traj_points.at(*end_idx).pose;
+      }
+    }
+
+    std::string uuid;
+    double target_vel;
+    double precise_lat_dist;
+    std::optional<geometry_msgs::msg::Pose> start_point{std::nullopt};
+    std::optional<geometry_msgs::msg::Pose> end_point{std::nullopt};
+  };
+  double calculateSlowDownVelocity(
+    const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const;
+  std::tuple<double, double, double> calculateDistanceToSlowDownWithConstraints(
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & traj_points,
-    const SlowDownObstacle & obstacle, const double dist_to_ego, const double slow_down_vel) const;
+    const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
+    const double dist_to_ego) const;
 
   struct SlowDownInfo
   {
@@ -183,6 +211,10 @@ class PlannerInterface
       min_ego_velocity = node.declare_parameter<double>("slow_down.min_ego_velocity");
       time_margin_on_target_velocity =
         node.declare_parameter<double>("slow_down.time_margin_on_target_velocity");
+      lpf_gain_slow_down_vel = node.declare_parameter<double>("slow_down.lpf_gain_slow_down_vel");
+      lpf_gain_lat_dist = node.declare_parameter<double>("slow_down.lpf_gain_lat_dist");
+      lpf_gain_dist_to_slow_down =
+        node.declare_parameter<double>("slow_down.lpf_gain_dist_to_slow_down");
     }
 
     void onParam(const std::vector<rclcpp::Parameter> & parameters)
@@ -197,15 +229,26 @@ class PlannerInterface
         parameters, "slow_down.min_ego_velocity", min_ego_velocity);
       tier4_autoware_utils::updateParam<double>(
         parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity);
+      tier4_autoware_utils::updateParam<double>(
+        parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel);
+      tier4_autoware_utils::updateParam<double>(
+        parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist);
+      tier4_autoware_utils::updateParam<double>(
+        parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down);
     }
 
     double max_lat_margin;
     double min_lat_margin;
     double max_ego_velocity;
     double min_ego_velocity;
     double time_margin_on_target_velocity;
+    double lpf_gain_slow_down_vel;
+    double lpf_gain_lat_dist;
+    double lpf_gain_dist_to_slow_down;
   };
   SlowDownParam slow_down_param_;
+
+  std::vector<SlowDownOutput> prev_slow_down_output_;
 };
 
 #endif  // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_

planning/obstacle_cruise_planner/src/node.cpp

@@ -962,9 +962,13 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
   // check lateral distance considering hysteresis
   const bool is_lat_dist_low = isLowerConsideringHysteresis(
     precise_lat_dist, is_prev_obstacle_slow_down,
-    p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down,
-    p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down);
+    p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0,
+    p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0);
   if (!is_lat_dist_low) {
+    RCLCPP_INFO_EXPRESSION(
+      get_logger(), enable_debug_info_,
+      "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(),
+      precise_lat_dist);
     return std::nullopt;
   }
 
@@ -1171,7 +1175,6 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData(
   planner_data.ego_vel = ego_odom_ptr_->twist.twist.linear.x;
   planner_data.ego_acc = ego_accel_ptr_->accel.accel.linear.x;
   planner_data.is_driving_forward = is_driving_forward_;
-
   return planner_data;
 }