feat(obstacle_cruise_planner)!: ignore to garze against unknwon objec…

…ts (autowarefoundation#7177) Signed-off-by: Yuki Takagi <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]>
tier4 · Jun 5, 2024 · 3ed88d2 · 3ed88d2
1 parent f3d3197
commit 3ed88d2
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 5 deletions.
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -85,7 +85,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.0 # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 0.0 # 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]
 

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -195,6 +195,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;

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -271,6 +271,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
 
   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");
   max_lat_margin_for_slow_down =
@@ -334,6 +336,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
 
   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<double>(
@@ -675,8 +680,8 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
     }();
     const auto & p = behavior_determination_param_;
     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_,
@@ -1009,7 +1014,13 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
   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;
   }
 
@@ -1064,7 +1075,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
 
   // calculate collision points with trajectory with lateral stop margin
   const auto traj_polys_with_lat_margin = createOneStepPolygons(
-    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
+    traj_points, vehicle_info_, ego_odom_ptr_->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_);