From 05e307118df971027992570576cd2fb8c709517e Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 31 May 2024 16:35:02 +0900 Subject: [PATCH] feat(obstacle_cruise_planner)!: ignore to garze against unknwon objects (#7177) Signed-off-by: Yuki Takagi Co-authored-by: Takayuki Murooka --- .../config/obstacle_cruise_planner.param.yaml | 3 ++- .../include/obstacle_cruise_planner/node.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 23 ++++++++++++++----- 3 files changed, 20 insertions(+), 7 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 index bd9c5750af78f..2ccd000657948 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/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] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 3f18c6e1728c6..fd65446408db1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/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; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 21e3ecb7bf758..c5f0c73a13b78 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -247,6 +247,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara max_lat_margin_for_stop = node.declare_parameter("behavior_determination.stop.max_lat_margin"); + max_lat_margin_for_stop_against_unknown = + node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); max_lat_margin_for_cruise = node.declare_parameter("behavior_determination.cruise.max_lat_margin"); enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); @@ -311,6 +313,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin_against_unknown", + max_lat_margin_for_stop_against_unknown); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( @@ -682,8 +687,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( }(); 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,7 +1179,13 @@ std::optional 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; } @@ -1207,7 +1218,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( 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_, @@ -1232,8 +1243,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } // 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_);