Skip to content

Commit

Permalink
feat(avoidance): change lateral margin based on if it's parked vehicle
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Feb 29, 2024
1 parent 398c47b commit f0ceab7
Show file tree
Hide file tree
Showing 10 changed files with 132 additions and 73 deletions.
48 changes: 32 additions & 16 deletions .../behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,64 +12,80 @@
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]
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
truck:
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
bus:
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
trailer:
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
unknown:
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.0
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
bicycle:
execute_num: 2
moving_speed_threshold: 0.28 # 1.0km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
motorcycle:
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
pedestrian:
execute_num: 2
moving_speed_threshold: 0.28 # 1.0km/h
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
lateral_margin:
soft_margin: 0.0 # [m]
hard_margin: 0.0 # [m]
hard_margin_for_parked_vehicle: 0.0 # [m]
lower_distance_for_polygon_expansion: 0.0 # [m]
upper_distance_for_polygon_expansion: 1.0 # [m]

Expand Down
10 changes: 6 additions & 4 deletions planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
param.envelope_buffer_margin =
getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
param.avoid_margin_lateral =
getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
param.safety_buffer_lateral =
getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
param.lateral_soft_margin =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
param.lateral_hard_margin =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
param.lateral_hard_margin_for_parked_vehicle =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");

Check warning on line 80 in planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

AvoidanceByLaneChangeModuleManager::init increases from 123 to 125 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
return param;
};

Expand Down
12 changes: 8 additions & 4 deletions planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,9 +257,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
const auto nearest_object_parameter =
avoidance_parameters_->object_parameters.at(nearest_object_type);
const auto avoid_margin =
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
const auto lateral_hard_margin = std::max(
nearest_object_parameter.lateral_hard_margin,
nearest_object_parameter.lateral_hard_margin_for_parked_vehicle);
const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor +
nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width;

avoidance_helper_->setData(planner_data_);
const auto shift_length = avoidance_helper_->getShiftLength(
Expand Down Expand Up @@ -288,8 +290,10 @@ double AvoidanceByLaneChange::calcLateralOffset() const
{
auto additional_lat_offset{0.0};
for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
const auto lateral_hard_margin =
std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle);
const auto offset =
2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin;
additional_lat_offset = std::max(additional_lat_offset, offset);
}
return additional_lat_offset;
Expand Down
72 changes: 44 additions & 28 deletions planning/behavior_path_avoidance_module/config/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,81 +30,97 @@
car:
execute_num: 1 # [-]
moving_speed_threshold: 1.0 # [m/s]
moving_time_threshold: 1.0 # [s]
moving_time_threshold: 2.0 # [s]
lateral_margin:
soft_margin: 0.7 # [m]
hard_margin: 0.3 # [m]
hard_margin_for_parked_vehicle: 0.3 # [m]
max_expand_ratio: 0.0 # [-]
envelope_buffer_margin: 0.3 # [m]
avoid_margin_lateral: 1.0 # [m]
safety_buffer_lateral: 0.7 # [m]
envelope_buffer_margin: 0.5 # [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:
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
moving_time_threshold: 2.0
lateral_margin:
soft_margin: 0.9 # [m]
hard_margin: 0.1 # [m]
hard_margin_for_parked_vehicle: 0.1 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bus:
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
moving_time_threshold: 2.0
lateral_margin:
soft_margin: 0.9 # [m]
hard_margin: 0.1 # [m]
hard_margin_for_parked_vehicle: 0.1 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
trailer:
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
moving_time_threshold: 2.0
lateral_margin:
soft_margin: 0.9 # [m]
hard_margin: 0.1 # [m]
hard_margin_for_parked_vehicle: 0.1 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
unknown:
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
lateral_margin:
soft_margin: 0.7 # [m]
hard_margin: -0.2 # [m]
hard_margin_for_parked_vehicle: -0.2 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.3
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
envelope_buffer_margin: 0.1
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bicycle:
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
lateral_margin:
soft_margin: 0.7 # [m]
hard_margin: 0.5 # [m]
hard_margin_for_parked_vehicle: 0.5 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
motorcycle:
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
lateral_margin:
soft_margin: 0.7 # [m]
hard_margin: 0.3 # [m]
hard_margin_for_parked_vehicle: 0.3 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
pedestrian:
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
lateral_margin:
soft_margin: 0.7 # [m]
hard_margin: 0.5 # [m]
hard_margin_for_parked_vehicle: 0.5 # [m]
max_expand_ratio: 0.0
envelope_buffer_margin: 0.8
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
envelope_buffer_margin: 0.5
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
lower_distance_for_polygon_expansion: 30.0 # [m]
Expand Down
6 changes: 4 additions & 2 deletions ...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,11 @@ struct ObjectParameter

double envelope_buffer_margin{0.0};

double avoid_margin_lateral{1.0};
double lateral_soft_margin{1.0};

double safety_buffer_lateral{1.0};
double lateral_hard_margin{1.0};

double lateral_hard_margin_for_parked_vehicle{1.0};

double safety_buffer_longitudinal{0.0};

Expand Down
10 changes: 6 additions & 4 deletions ...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
param.envelope_buffer_margin =
getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
param.avoid_margin_lateral =
getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
param.safety_buffer_lateral =
getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
param.lateral_soft_margin =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
param.lateral_hard_margin =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
param.lateral_hard_margin_for_parked_vehicle =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
param.safety_buffer_longitudinal =
getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
param.use_conservative_buffer_longitudinal =
Expand Down
7 changes: 5 additions & 2 deletions planning/behavior_path_avoidance_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,11 @@ 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 + "lateral_margin.soft_margin", config.lateral_soft_margin);
updateParam<double>(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin);
updateParam<double>(
parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle",
config.lateral_hard_margin_for_parked_vehicle);

Check warning on line 67 in planning/behavior_path_avoidance_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

AvoidanceModuleManager::updateModuleParams already has high cyclomatic complexity, and now it increases in Lines of Code from 106 to 109. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
updateParam<double>(
parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
updateParam<bool>(
Expand Down
12 changes: 6 additions & 6 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -635,8 +635,8 @@ void AvoidanceModule::fillDebugData(
: 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 +
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
object_parameter.lateral_soft_margin + 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 @@ -1423,8 +1423,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);

const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
const auto variable = helper_->getMinAvoidanceDistance(
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
const auto & additional_buffer_longitudinal =
Expand Down Expand Up @@ -1653,8 +1653,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
const auto shift_length =
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);

Expand Down
5 changes: 4 additions & 1 deletion planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,9 +234,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(

const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;

const auto lateral_hard_margin = object.is_parked
? object_parameter.lateral_hard_margin_for_parked_vehicle
: object_parameter.lateral_hard_margin;
const auto infeasible =
std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
0.5 * data_->parameters.vehicle_width + lateral_hard_margin;

Check warning on line 242 in planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

ShiftLineGenerator::generateAvoidOutline increases in cyclomatic complexity from 49 to 50, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
if (infeasible) {
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
Expand Down
Loading

0 comments on commit f0ceab7

Please sign in to comment.