Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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 <satoshi.ota928@gmail.com>
@satoshi-ota
satoshi-ota committed Mar 5, 2024
1 parent c3093d2 commit ba7b435
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
@@ -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]

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
@@ -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

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;
};

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
@@ -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;

Check warning on line 264 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L262-L264 

Added lines #L262 - L264 were not covered by tests

avoidance_helper_->setData(planner_data_);
const auto shift_length = avoidance_helper_->getShiftLength(
@@ -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;
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
@@ -27,81 +27,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]
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
@@ -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};

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
@@ -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 =
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
@@ -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

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>(
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
@@ -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;

Check warning on line 639 in planning/behavior_path_avoidance_module/src/scene.cpp

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L638-L639 

Added lines #L638 - L639 were not covered by tests

const auto variable = helper_->getSharpAvoidanceDistance(
helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
@@ -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;

Check warning on line 1427 in planning/behavior_path_avoidance_module/src/scene.cpp

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L1426-L1427 

Added lines #L1426 - L1427 were not covered by tests
const auto variable = helper_->getMinAvoidanceDistance(
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
const auto & additional_buffer_longitudinal =
@@ -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 +

Check warning on line 1656 in planning/behavior_path_avoidance_module/src/scene.cpp

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L1656 

Added line #L1656 was not covered by tests
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
const auto shift_length =
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);

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
@@ -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

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

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp#L237 

Added line #L237 was not covered by tests
? 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

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.

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

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp#L242 

Added line #L242 was not covered by tests
if (infeasible) {
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
Loading

0 comments on commit ba7b435

Please sign in to comment.