feat(obstacle_cruise_planner): hysteresis for slow down decision

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -86,14 +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.5 # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 0.8 # 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: 1.0  # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 0.7  # lateral margin between obstacle and trajectory band with ego's width
+        lat_hysteresis_margin: 0.1
 
     cruise:
       pid_based_planner:

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -185,6 +185,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double max_lat_margin_for_stop;
     double max_lat_margin_for_cruise;
     double max_lat_margin_for_slow_down;
+    double lat_hysteresis_margin_for_slow_down;
   };
   BehaviorDeterminationParam behavior_determination_param_;
 

planning/obstacle_cruise_planner/src/node.cpp

@@ -202,6 +202,21 @@ geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & poin
   geom_point.y = point.y();
   return geom_point;
 }
+
+bool isLowerConsideringHysteresis(
+  const double current_val, const bool was_low, const double high_val, const double low_val)
+{
+  if (was_low) {
+    if (high_val < current_val) {
+      return false;
+    }
+    return true;
+  }
+  if (current_val < low_val) {
+    return true;
+  }
+  return false;
+}
 }  // namespace
 
 namespace motion_planning
@@ -244,6 +259,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     node.declare_parameter<double>("behavior_determination.cruise.max_lat_margin");
   max_lat_margin_for_slow_down =
     node.declare_parameter<double>("behavior_determination.slow_down.max_lat_margin");
+  lat_hysteresis_margin_for_slow_down =
+    node.declare_parameter<double>("behavior_determination.slow_down.lat_hysteresis_margin");
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -293,6 +310,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
     parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.slow_down.lat_hysteresis_margin",
+    lat_hysteresis_margin_for_slow_down);
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -733,8 +753,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle(
 
   {  // consider hysteresis
     const double obstacle_projected_vel = calcObstacleProjectedVelocity(traj_points, obstacle);
-    const auto is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, obstacle.uuid);
-    const auto is_prev_obstacle_cruise = getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid);
+    // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_,
+    // obstacle.uuid).has_value();
+    const bool is_prev_obstacle_cruise =
+      getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid).has_value();
 
     if (is_prev_obstacle_cruise) {
       if (obstacle_projected_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) {
@@ -930,9 +952,19 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
   const auto & object_id = obstacle.uuid.substr(0, 4);
   const auto & p = behavior_determination_param_;
 
-  if (
-    !enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label) ||
-    p.max_lat_margin_for_slow_down < precise_lat_dist) {
+  const bool is_prev_obstacle_slow_down =
+    getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value();
+
+  if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) {
+    return std::nullopt;
+  }
+
+  // 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);
+  if (!is_lat_dist_low) {
     return std::nullopt;
   }