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): add use_conservative_buffer_longitudinal #983

Merged
merged 1 commit into from
Oct 31, 2023
Merged

feat(avoidance): add use_conservative_buffer_longitudinal #983

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
8 changes: 8 additions & 0 deletions 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 @@ -37,6 +37,7 @@
avoid_margin_lateral: 1.0 # [m]
safety_buffer_lateral: 0.7 # [m]
safety_buffer_longitudinal: 0.0 # [m]
use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
truck:
is_target: true
execute_num: 1
Expand All @@ -47,6 +48,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bus:
is_target: true
execute_num: 1
Expand All @@ -57,6 +59,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
trailer:
is_target: true
execute_num: 1
Expand All @@ -67,6 +70,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
unknown:
is_target: false
execute_num: 1
Expand All @@ -77,6 +81,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bicycle:
is_target: false
execute_num: 1
Expand All @@ -87,6 +92,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
motorcycle:
is_target: false
execute_num: 1
Expand All @@ -97,6 +103,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
pedestrian:
is_target: false
execute_num: 1
Expand All @@ -107,6 +114,7 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
lower_distance_for_polygon_expansion: 30.0 # [m]
upper_distance_for_polygon_expansion: 100.0 # [m]

Expand Down
2 changes: 2 additions & 0 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 @@ -69,6 +69,8 @@ struct ObjectParameter
double safety_buffer_lateral{1.0};

double safety_buffer_longitudinal{0.0};

bool use_conservative_buffer_longitudinal{true};
};

struct AvoidanceParameters
Expand Down
34 changes: 25 additions & 9 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 @@ -542,7 +542,10 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
const auto o_front = data.stop_target_object.get();
const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto & base_link2front = planner_data_->parameters.base_link2front;
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto & vehicle_width = planner_data_->parameters.vehicle_width;

const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
Expand All @@ -551,7 +554,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
const auto variable = helper_.getSharpAvoidanceDistance(
helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
const auto constant = helper_.getNominalPrepareDistance() +
object_parameter.safety_buffer_longitudinal + base_link2front;
object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal;
const auto total_avoid_distance = variable + constant;

dead_pose_ = calcLongitudinalOffsetPose(
Expand Down Expand Up @@ -796,7 +800,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
{
// To be consistent with changes in the ego position, the current shift length is considered.
const auto current_ego_shift = helper_.getEgoShift();
const auto & base_link2front = planner_data_->parameters.base_link2front;
const auto & base_link2rear = planner_data_->parameters.base_link2rear;

// Calculate feasible shift length
Expand Down Expand Up @@ -834,8 +837,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(

// calculate remaining distance.
const auto prepare_distance = helper_.getNominalPrepareDistance();
const auto constant =
object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance;
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant = object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal + prepare_distance;
const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
const auto remaining_distance = object.longitudinal - constant;

Expand Down Expand Up @@ -955,7 +962,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(

AvoidLine al_avoid;
{
const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front;
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto offset =
object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
const auto path_front_to_ego =
avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index);

Expand Down Expand Up @@ -2727,7 +2739,6 @@ void AvoidanceModule::updateAvoidanceDebugData(
double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
{
const auto & p = parameters_;
const auto & base_link2front = planner_data_->parameters.base_link2front;
const auto & vehicle_width = planner_data_->parameters.vehicle_width;

// D5
Expand All @@ -2743,7 +2754,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
// D2: min_avoid_distance
// D3: longitudinal_avoid_margin_front (margin + D5)
// D4: o_front.longitudinal
// D5: base_link2front
// D5: additional_buffer_longitudinal (base_link2front or 0 depending on the
// use_conservative_buffer_longitudinal)

const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
Expand All @@ -2752,8 +2764,12 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const auto variable = helper_.getMinAvoidanceDistance(
helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
base_link2front + p->stop_buffer;
additional_buffer_longitudinal + p->stop_buffer;

return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
}
Expand Down
5 changes: 5 additions & 0 deletions 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 @@ -79,6 +79,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
param.safety_buffer_longitudinal =
getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
param.use_conservative_buffer_longitudinal =
getOrDeclareParameter<bool>(*node, ns + "use_conservative_buffer_longitudinal");
return param;
};

Expand Down Expand Up @@ -312,6 +314,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
updateParam<double>(
parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
updateParam<bool>(
parameters, ns + "use_conservative_buffer_longitudinal",
config.use_conservative_buffer_longitudinal);
};

{
Expand Down
Loading