Skip to content

Commit

Permalink
add filter
Browse files Browse the repository at this point in the history
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 committed Jun 5, 2024
1 parent 3ed88d2 commit 221325e
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -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]

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
8 changes: 7 additions & 1 deletion planning/obstacle_cruise_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -273,6 +273,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
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_stop_against_unknown_height_threshold = node.declare_parameter<double>(
"behavior_determination.stop.max_lat_margin_against_unknown_height_threshold");
max_lat_margin_for_cruise =
node.declare_parameter<double>("behavior_determination.cruise.max_lat_margin");
max_lat_margin_for_slow_down =
Expand Down Expand Up @@ -339,6 +341,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
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.stop.max_lat_margin_against_unknown_height_threshold",
max_lat_margin_for_stop_against_unknown_height_threshold);
tier4_autoware_utils::updateParam<double>(
parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
tier4_autoware_utils::updateParam<double>(
Expand Down Expand Up @@ -1016,7 +1021,8 @@ std::optional<StopObstacle> 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;

Expand Down

0 comments on commit 221325e

Please sign in to comment.