From c3e1d7f61eea689f9eed587bd508cd9ef746096a Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 4 Jun 2024 17:20:19 +0900 Subject: [PATCH] add filter Signed-off-by: Yuki Takagi --- .../config/obstacle_cruise_planner.param.yaml | 1 + .../include/obstacle_cruise_planner/node.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 8 +++++++- 3 files changed, 9 insertions(+), 1 deletion(-) 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 2ccd000657948..788bae29d8384 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -89,6 +89,7 @@ stop: 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 + max_lat_margin_against_unknown_height_threshold: 0.6 # Only for unknowns lower than this height, the parameter above for unknowns is referenced. 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 fd65446408db1..c8c074827873e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -201,6 +201,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // max lateral margin double max_lat_margin_for_stop; double max_lat_margin_for_stop_against_unknown; + double max_lat_margin_for_stop_against_unknown_height_threshold; 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 c5f0c73a13b78..ca5dfd52eed1f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -249,6 +249,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara 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_stop_against_unknown_height_threshold = node.declare_parameter( + "behavior_determination.stop.max_lat_margin_against_unknown_height_threshold"); 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"); @@ -316,6 +318,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( 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.stop.max_lat_margin_against_unknown_height_threshold", + max_lat_margin_for_stop_against_unknown_height_threshold); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( @@ -1181,7 +1186,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const double max_lat_margin_for_stop = - (obstacle.classification.label == ObjectClassification::UNKNOWN) + (obstacle.classification.label == ObjectClassification::UNKNOWN && + obstacle.shape.dimensions.z < p.max_lat_margin_for_stop_against_unknown_height_threshold) ? p.max_lat_margin_for_stop_against_unknown : p.max_lat_margin_for_stop;