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): change lateral margin based on if it's parked vehicle #6520

Merged
merged 2 commits into from
Mar 7, 2024
Merged

feat(avoidance): change lateral margin based on if it's parked vehicle #6520

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c3093d2
feat(avoidance): check if it's parked vehicle
satoshi-ota Feb 29, 2024
ba7b435
feat(avoidance): change lateral margin based on if it's parked vehicle
satoshi-ota Feb 29, 2024
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
48 changes: 32 additions & 16 deletions .../behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml
View file
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,12 @@
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -257,9 +257,11 @@
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

View check run for this annotation

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(
Expand Down Expand Up @@ -288,8 +290,10 @@
{
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
9 changes: 7 additions & 2 deletions ...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
View file
Open in desktop
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 Expand Up @@ -415,6 +417,9 @@ struct ObjectData // avoidance target
// is within intersection area
bool is_within_intersection{false};

// is parked vehicle on road shoulder
bool is_parked{false};

// object direction.
Direction direction{Direction::NONE};

Expand Down
10 changes: 6 additions & 4 deletions ...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion planning/behavior_path_avoidance_module/src/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
marker.id = uuidToInt32(object.object.object_id);
marker.pose.position.z += 2.0;
std::ostringstream string_stream;
string_stream << object.reason;
string_stream << object.reason << (object.is_parked ? "(PARKED)" : "");
marker.text = string_stream.str();
marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999);
marker.scale = createMarkerScale(0.6, 0.6, 0.6);
Expand Down
7 changes: 5 additions & 2 deletions planning/behavior_path_avoidance_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,11 @@
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -635,8 +635,8 @@
: 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

View check run for this annotation

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));
Expand Down Expand Up @@ -1423,8 +1423,8 @@
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

View check run for this annotation

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 =
Expand Down Expand Up @@ -1653,8 +1653,8 @@
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

View check run for this annotation

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

Expand Down
5 changes: 4 additions & 1 deletion planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 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: Overall Code Complexity 

The mean cyclomatic complexity increases from 8.28 to 8.32, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -234,9 +234,12 @@

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

View check run for this annotation

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

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.

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

View check run for this annotation

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;
Expand Down
Loading
Loading