feat(obstacle_cruise_planner)!: ignore to graze against unknwon objects

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -87,7 +87,8 @@
         obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
 
       stop:
-        max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint
+        max_lat_margin_against_unknown: -0.3 # lateral margin between the unknown obstacles and ego's footprint
         crossing_obstacle:
           collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -200,6 +200,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double prediction_resampling_time_horizon;
     // max lateral margin
     double max_lat_margin_for_stop;
+    double max_lat_margin_for_stop_against_unknown;
     double max_lat_margin_for_cruise;
     double max_lat_margin_for_slow_down;
     double lat_hysteresis_margin_for_slow_down;

planning/obstacle_cruise_planner/src/node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -247,6 +247,8 @@
 
   max_lat_margin_for_stop =
     node.declare_parameter<double>("behavior_determination.stop.max_lat_margin");
+  max_lat_margin_for_stop_against_unknown =
+    node.declare_parameter<double>("behavior_determination.stop.max_lat_margin_against_unknown");
   max_lat_margin_for_cruise =
     node.declare_parameter<double>("behavior_determination.cruise.max_lat_margin");
   enable_yield = node.declare_parameter<bool>("behavior_determination.cruise.yield.enable_yield");
@@ -311,6 +313,9 @@
 
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.stop.max_lat_margin_against_unknown",
+    max_lat_margin_for_stop_against_unknown);
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
   tier4_autoware_utils::updateParam<bool>(
@@ -682,8 +687,8 @@
     }();
 
     const double max_lat_margin = std::max(
-      std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise),
-      p.max_lat_margin_for_slow_down);
+      {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown,
+       p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down});
     if (max_lat_margin < min_lat_dist_to_traj_poly) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), enable_debug_info_,
@@ -1174,66 +1179,72 @@
   if (!isStopObstacle(obstacle.classification.label)) {
     return std::nullopt;
   }
-  if (p.max_lat_margin_for_stop < precise_lat_dist) {
+
+  const double max_lat_margin_for_stop =
+    (obstacle.classification.label == ObjectClassification::UNKNOWN)
+      ? p.max_lat_margin_for_stop_against_unknown
+      : p.max_lat_margin_for_stop;
+
+  if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) {
     return std::nullopt;
   }
 
   // check obstacle velocity
   // NOTE: If precise_lat_dist is 0, always plan stop
   constexpr double epsilon = 1e-6;
   if (epsilon < precise_lat_dist) {
     const auto [obstacle_tangent_vel, obstacle_normal_vel] =
       projectObstacleVelocityToTrajectory(traj_points, obstacle);
     if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) {
       return std::nullopt;
     }
   }
 
   // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed,
   //       and the collision between ego and obstacles are within the margin threshold.
   const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle);
   const double has_high_speed = p.crossing_obstacle_velocity_threshold <
                                 std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y);
   if (is_obstacle_crossing && has_high_speed) {
     // Get highest confidence predicted path
     const auto resampled_predicted_path = resampleHighestConfidencePredictedPath(
       obstacle.predicted_paths, p.prediction_resampling_time_interval,
       p.prediction_resampling_time_horizon);
 
     std::vector<size_t> collision_index;
     const auto collision_points = polygon_utils::getCollisionPoints(
       traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
       is_driving_forward_, collision_index,
       calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length +
         std::hypot(
           vehicle_info_.vehicle_length_m,
-          vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop));
+          vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop));
     if (collision_points.empty()) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), enable_debug_info_,
         "[Stop] Ignore inside obstacle (%s) since there is no collision point between the "
         "predicted path "
         "and the ego.",
         object_id.c_str());
       debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle);
       return std::nullopt;
     }
 
     const double collision_time_margin =
       calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_);
     if (p.collision_time_margin < collision_time_margin) {
       RCLCPP_INFO_EXPRESSION(
         get_logger(), enable_debug_info_,
         "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.",
         object_id.c_str());
       debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle);
       return std::nullopt;
     }
   }
 
   // calculate collision points with trajectory with lateral stop margin
-  const auto traj_polys_with_lat_margin = createOneStepPolygons(
-    traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop);
+  const auto traj_polys_with_lat_margin =
+    createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop);
 
   const auto collision_point = polygon_utils::getCollisionPoint(
     traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);