From 221325e17017849109119497140828a1de90f82f 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 bd80a017dd5bd..adc49d1c4e729 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -87,6 +87,7 @@ stop: 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 + 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 4940e083e45d1..b72c7e1df25ba 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -196,6 +196,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 702fe13579918..90a45bba042b0 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -273,6 +273,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"); max_lat_margin_for_slow_down = @@ -339,6 +341,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( @@ -1016,7 +1021,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;