Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(avoidance): set additional buffer margin independently #4041

Merged
merged 1 commit into from
Jun 26, 2023
Merged

feat(avoidance): set additional buffer margin independently #4041

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 8 additions & 1 deletion planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
moving_time_threshold: 1.0 # [s]
max_expand_ratio: 0.0 # [-]
envelope_buffer_margin: 0.3 # [m]
avoid_margin_lateral: 1.0 # [m]
safety_buffer_lateral: 0.7 # [m]
safety_buffer_longitudinal: 0.0 # [m]
truck:
Expand All @@ -41,6 +42,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
bus:
Expand All @@ -49,6 +51,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
trailer:
Expand All @@ -57,6 +60,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
unknown:
Expand All @@ -65,6 +69,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
bicycle:
Expand All @@ -73,6 +78,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
motorcycle:
Expand All @@ -81,6 +87,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
pedestrian:
Expand All @@ -89,6 +96,7 @@
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
lower_distance_for_polygon_expansion: 30.0 # [m]
Expand Down Expand Up @@ -124,7 +132,6 @@
avoidance:
# avoidance lateral parameters
lateral:
lateral_collision_margin: 1.0 # [m]
lateral_execution_threshold: 0.499 # [m]
lateral_small_shift_threshold: 0.101 # [m]
road_shoulder_safety_margin: 0.3 # [m]
Expand Down
5 changes: 2 additions & 3 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@ struct ObjectParameter

double envelope_buffer_margin{0.0};

double avoid_margin_lateral{1.0};

double safety_buffer_lateral{1.0};

double safety_buffer_longitudinal{0.0};
Expand Down Expand Up @@ -159,9 +161,6 @@ struct AvoidanceParameters
// force avoidance
double threshold_time_force_avoidance_for_stopped_vehicle;

// we want to keep this lateral margin when avoiding
double lateral_collision_margin;

// when complete avoidance motion, there is a distance margin with the object
// for longitudinal direction
double longitudinal_collision_margin_min_distance;
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -586,6 +586,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
param.moving_time_threshold = declare_parameter<double>(ns + "moving_time_threshold");
param.max_expand_ratio = declare_parameter<double>(ns + "max_expand_ratio");
param.envelope_buffer_margin = declare_parameter<double>(ns + "envelope_buffer_margin");
param.avoid_margin_lateral = declare_parameter<double>(ns + "avoid_margin_lateral");
param.safety_buffer_lateral = declare_parameter<double>(ns + "safety_buffer_lateral");
param.safety_buffer_longitudinal =
declare_parameter<double>(ns + "safety_buffer_longitudinal");
Expand Down Expand Up @@ -649,7 +650,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
// avoidance maneuver (lateral)
{
std::string ns = "avoidance.avoidance.lateral.";
p.lateral_collision_margin = declare_parameter<double>(ns + "lateral_collision_margin");
p.road_shoulder_safety_margin = declare_parameter<double>(ns + "road_shoulder_safety_margin");
p.lateral_execution_threshold = declare_parameter<double>(ns + "lateral_execution_threshold");
p.lateral_small_shift_threshold =
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -620,7 +620,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
const auto & vehicle_width = planner_data_->parameters.vehicle_width;

const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
parameters_->lateral_collision_margin + 0.5 * vehicle_width;
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;

const auto variable = helper_.getSharpAvoidanceDistance(
helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
Expand Down Expand Up @@ -3285,7 +3285,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
const auto object_parameter = parameters_->object_parameters.at(t);

const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
p->lateral_collision_margin + 0.5 * vehicle_width;
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const auto variable = helper_.getMinimumAvoidanceDistance(
helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
const auto constant =
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);
updateParam<double>(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin);
updateParam<double>(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral);
updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
updateParam<double>(
parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
Expand Down Expand Up @@ -82,7 +83,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold);
updateParam<double>(
parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold);
updateParam<double>(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin);
updateParam<double>(
parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin);
}
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -894,7 +894,7 @@ void filterTargetObjects(
// calculate avoid_margin dynamically
// NOTE: This calculation must be after calculating to_road_shoulder_distance.
const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +
parameters->lateral_collision_margin + 0.5 * vehicle_width;
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const double min_safety_lateral_distance =
object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
const auto max_allowable_lateral_distance =
Expand Down