set hysteresis-based obstacle moving classification

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -49,6 +49,11 @@ class PlannerInterface
       node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
       node.create_publisher<StopSpeedExceeded>("~/output/stop_speed_exceeded", 1);
+
+    moving_object_speed_threshold =
+      node.declare_parameter<double>("slow_down.moving_object_speed_threshold");
+    moving_object_hysteresis_range =
+      node.declare_parameter<double>("slow_down.moving_object_hysteresis_range");
   }
 
   PlannerInterface() = default;
@@ -183,11 +188,12 @@ class PlannerInterface
       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_feasible_target_vel,
-      const double arg_precise_lat_dist)
+      const double arg_precise_lat_dist, const bool is_moving)
     : uuid(arg_uuid),
       target_vel(arg_target_vel),
       feasible_target_vel(arg_feasible_target_vel),
-      precise_lat_dist(arg_precise_lat_dist)
+      precise_lat_dist(arg_precise_lat_dist),
+      is_moving(is_moving)
     {
       if (start_idx) {
         start_point = traj_points.at(*start_idx).pose;
@@ -203,15 +209,17 @@ class PlannerInterface
     double precise_lat_dist;
     std::optional<geometry_msgs::msg::Pose> start_point{std::nullopt};
     std::optional<geometry_msgs::msg::Pose> end_point{std::nullopt};
+    bool is_moving;
   };
   double calculateMarginFromObstacleOnCurve(
     const PlannerData & planner_data, const StopObstacle & stop_obstacle) const;
   double calculateSlowDownVelocity(
-    const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const;
+    const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
+    bool & is_obstacle_moving) const;
   std::optional<std::tuple<double, double, double>> calculateDistanceToSlowDownWithConstraints(
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & traj_points,
     const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
-    const double dist_to_ego) const;
+    const double dist_to_ego, bool & is_obstacle_moving) const;
 
   struct SlowDownInfo
   {
@@ -290,7 +298,7 @@ class PlannerInterface
         for (const auto & movement_postfix : obstacle_moving_classification) {
           if (obstacle_to_param_struct_map.count(label + "_" + movement_postfix) < 1) continue;
           auto & param_by_obstacle_label =
-            obstacle_to_param_struct_map.at(movement_postfix + "_" + label);
+            obstacle_to_param_struct_map.at(label + "_" + movement_postfix);
           tier4_autoware_utils::updateParam<double>(
             parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin",
             param_by_obstacle_label.max_lat_margin);
@@ -325,7 +333,8 @@ class PlannerInterface
     double lpf_gain_dist_to_slow_down;
   };
   SlowDownParam slow_down_param_;
-
+  double moving_object_speed_threshold;
+  double moving_object_hysteresis_range;
   std::vector<SlowDownOutput> prev_slow_down_output_;
   // previous trajectory and distance to stop
   // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -562,8 +562,9 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
     const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid);
 
     // calculate slow down start distance, and insert slow down velocity
+    bool is_obstacle_moving;
     const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints(
-      planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego);
+      planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, is_obstacle_moving);
     if (!dist_vec_to_slow_down) {
       RCLCPP_INFO_EXPRESSION(
         rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_,
@@ -648,7 +649,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
     // update prev_slow_down_output_
     new_prev_slow_down_output.push_back(SlowDownOutput{
       obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx,
-      stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist});
+      stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist, is_obstacle_moving});
   }
 
   // update prev_slow_down_output_
@@ -663,12 +664,20 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
 }
 
 double PlannerInterface::calculateSlowDownVelocity(
-  const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const
+  const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
+  bool & is_obstacle_moving) const
 {
-  const bool is_obstacle_moving = [&obstacle]() { return std::abs(obstacle.velocity) > 0.5; }();
+  is_obstacle_moving = [&]() -> bool {
+    if (!prev_output) return std::abs(obstacle.velocity) > moving_object_speed_threshold;
+    if (prev_output->is_moving)
+      return std::abs(obstacle.velocity) >
+             moving_object_speed_threshold - moving_object_hysteresis_range;
+    return std::abs(obstacle.velocity) >
+           moving_object_speed_threshold + moving_object_hysteresis_range;
+  }();
+
   const auto & p =
     slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving);
-  std::cerr << "Max ego vel " << p.min_ego_velocity << "\n";
   const double stable_precise_lat_dist = [&]() {
     if (prev_output) {
       return signal_processing::lowpassFilter(
@@ -691,15 +700,15 @@ std::optional<std::tuple<double, double, double>>
 PlannerInterface::calculateDistanceToSlowDownWithConstraints(
   const PlannerData & planner_data, const std::vector<TrajectoryPoint> & traj_points,
   const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
-  const double dist_to_ego) const
+  const double dist_to_ego, bool & is_obstacle_moving) const
 {
   const double abs_ego_offset = planner_data.is_driving_forward
                                   ? std::abs(vehicle_info_.max_longitudinal_offset_m)
                                   : std::abs(vehicle_info_.min_longitudinal_offset_m);
   const double obstacle_vel = obstacle.velocity;
 
   // calculate slow down velocity
-  const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output);
+  const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output, is_obstacle_moving);
 
   // calculate distance to collision points
   const double dist_to_front_collision =