Skip to content

Commit e1a3139

Browse files
satoshi-otasatoshi-ota
authored
feat(avoidance): change lateral margin based on if it's parked vehicle (#6520)
* feat(avoidance): check if it's parked vehicle Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): change lateral margin based on if it's parked vehicle Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 5206580 commit e1a3139

File tree

11 files changed

+158
-103
lines changed

11 files changed

+158
-103
lines changed

‎planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml

+32-16
Original file line numberDiff line numberDiff line change
@@ -12,64 +12,80 @@
1212
moving_time_threshold: 1.0 # [s]
1313
max_expand_ratio: 0.0 # [-]
1414
envelope_buffer_margin: 0.3 # [m]
15-
avoid_margin_lateral: 1.0 # [m]
16-
safety_buffer_lateral: 0.7 # [m]
15+
lateral_margin:
16+
soft_margin: 0.0 # [m]
17+
hard_margin: 0.0 # [m]
18+
hard_margin_for_parked_vehicle: 0.0 # [m]
1719
truck:
1820
execute_num: 2
1921
moving_speed_threshold: 1.0 # 3.6km/h
2022
moving_time_threshold: 1.0
2123
max_expand_ratio: 0.0
2224
envelope_buffer_margin: 0.3
23-
avoid_margin_lateral: 1.0
24-
safety_buffer_lateral: 0.7
25+
lateral_margin:
26+
soft_margin: 0.0 # [m]
27+
hard_margin: 0.0 # [m]
28+
hard_margin_for_parked_vehicle: 0.0 # [m]
2529
bus:
2630
execute_num: 2
2731
moving_speed_threshold: 1.0 # 3.6km/h
2832
moving_time_threshold: 1.0
2933
max_expand_ratio: 0.0
3034
envelope_buffer_margin: 0.3
31-
avoid_margin_lateral: 1.0
32-
safety_buffer_lateral: 0.7
35+
lateral_margin:
36+
soft_margin: 0.0 # [m]
37+
hard_margin: 0.0 # [m]
38+
hard_margin_for_parked_vehicle: 0.0 # [m]
3339
trailer:
3440
execute_num: 2
3541
moving_speed_threshold: 1.0 # 3.6km/h
3642
moving_time_threshold: 1.0
3743
max_expand_ratio: 0.0
3844
envelope_buffer_margin: 0.3
39-
avoid_margin_lateral: 1.0
40-
safety_buffer_lateral: 0.7
45+
lateral_margin:
46+
soft_margin: 0.0 # [m]
47+
hard_margin: 0.0 # [m]
48+
hard_margin_for_parked_vehicle: 0.0 # [m]
4149
unknown:
4250
execute_num: 1
4351
moving_speed_threshold: 0.28 # 1.0km/h
4452
moving_time_threshold: 1.0
4553
max_expand_ratio: 0.0
4654
envelope_buffer_margin: 0.3
47-
avoid_margin_lateral: 1.0
48-
safety_buffer_lateral: 0.0
55+
lateral_margin:
56+
soft_margin: 0.0 # [m]
57+
hard_margin: 0.0 # [m]
58+
hard_margin_for_parked_vehicle: 0.0 # [m]
4959
bicycle:
5060
execute_num: 2
5161
moving_speed_threshold: 0.28 # 1.0km/h
5262
moving_time_threshold: 1.0
5363
max_expand_ratio: 0.0
5464
envelope_buffer_margin: 0.8
55-
avoid_margin_lateral: 1.0
56-
safety_buffer_lateral: 1.0
65+
lateral_margin:
66+
soft_margin: 0.0 # [m]
67+
hard_margin: 0.0 # [m]
68+
hard_margin_for_parked_vehicle: 0.0 # [m]
5769
motorcycle:
5870
execute_num: 2
5971
moving_speed_threshold: 1.0 # 3.6km/h
6072
moving_time_threshold: 1.0
6173
max_expand_ratio: 0.0
6274
envelope_buffer_margin: 0.8
63-
avoid_margin_lateral: 1.0
64-
safety_buffer_lateral: 1.0
75+
lateral_margin:
76+
soft_margin: 0.0 # [m]
77+
hard_margin: 0.0 # [m]
78+
hard_margin_for_parked_vehicle: 0.0 # [m]
6579
pedestrian:
6680
execute_num: 2
6781
moving_speed_threshold: 0.28 # 1.0km/h
6882
moving_time_threshold: 1.0
6983
max_expand_ratio: 0.0
7084
envelope_buffer_margin: 0.8
71-
avoid_margin_lateral: 1.0
72-
safety_buffer_lateral: 1.0
85+
lateral_margin:
86+
soft_margin: 0.0 # [m]
87+
hard_margin: 0.0 # [m]
88+
hard_margin_for_parked_vehicle: 0.0 # [m]
7389
lower_distance_for_polygon_expansion: 0.0 # [m]
7490
upper_distance_for_polygon_expansion: 1.0 # [m]
7591

‎planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
7272
param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
7373
param.envelope_buffer_margin =
7474
getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
75-
param.avoid_margin_lateral =
76-
getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
77-
param.safety_buffer_lateral =
78-
getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
75+
param.lateral_soft_margin =
76+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
77+
param.lateral_hard_margin =
78+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
79+
param.lateral_hard_margin_for_parked_vehicle =
80+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
7981
return param;
8082
};
8183

‎planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+8-4
Original file line numberDiff line numberDiff line change
@@ -257,9 +257,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
257257
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
258258
const auto nearest_object_parameter =
259259
avoidance_parameters_->object_parameters.at(nearest_object_type);
260-
const auto avoid_margin =
261-
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
262-
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
260+
const auto lateral_hard_margin = std::max(
261+
nearest_object_parameter.lateral_hard_margin,
262+
nearest_object_parameter.lateral_hard_margin_for_parked_vehicle);
263+
const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor +
264+
nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width;
263265

264266
avoidance_helper_->setData(planner_data_);
265267
const auto shift_length = avoidance_helper_->getShiftLength(
@@ -288,8 +290,10 @@ double AvoidanceByLaneChange::calcLateralOffset() const
288290
{
289291
auto additional_lat_offset{0.0};
290292
for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
293+
const auto lateral_hard_margin =
294+
std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle);
291295
const auto offset =
292-
2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
296+
2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin;
293297
additional_lat_offset = std::max(additional_lat_offset, offset);
294298
}
295299
return additional_lat_offset;

‎planning/behavior_path_avoidance_module/config/avoidance.param.yaml

+44-28
Original file line numberDiff line numberDiff line change
@@ -27,81 +27,97 @@
2727
car:
2828
execute_num: 1 # [-]
2929
moving_speed_threshold: 1.0 # [m/s]
30-
moving_time_threshold: 1.0 # [s]
30+
moving_time_threshold: 2.0 # [s]
31+
lateral_margin:
32+
soft_margin: 0.7 # [m]
33+
hard_margin: 0.3 # [m]
34+
hard_margin_for_parked_vehicle: 0.3 # [m]
3135
max_expand_ratio: 0.0 # [-]
32-
envelope_buffer_margin: 0.3 # [m]
33-
avoid_margin_lateral: 1.0 # [m]
34-
safety_buffer_lateral: 0.7 # [m]
36+
envelope_buffer_margin: 0.5 # [m]
3537
safety_buffer_longitudinal: 0.0 # [m]
3638
use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
3739
truck:
3840
execute_num: 1
3941
moving_speed_threshold: 1.0 # 3.6km/h
40-
moving_time_threshold: 1.0
42+
moving_time_threshold: 2.0
43+
lateral_margin:
44+
soft_margin: 0.9 # [m]
45+
hard_margin: 0.1 # [m]
46+
hard_margin_for_parked_vehicle: 0.1 # [m]
4147
max_expand_ratio: 0.0
42-
envelope_buffer_margin: 0.3
43-
avoid_margin_lateral: 1.0
44-
safety_buffer_lateral: 0.7
48+
envelope_buffer_margin: 0.5
4549
safety_buffer_longitudinal: 0.0
4650
use_conservative_buffer_longitudinal: true
4751
bus:
4852
execute_num: 1
4953
moving_speed_threshold: 1.0 # 3.6km/h
50-
moving_time_threshold: 1.0
54+
moving_time_threshold: 2.0
55+
lateral_margin:
56+
soft_margin: 0.9 # [m]
57+
hard_margin: 0.1 # [m]
58+
hard_margin_for_parked_vehicle: 0.1 # [m]
5159
max_expand_ratio: 0.0
52-
envelope_buffer_margin: 0.3
53-
avoid_margin_lateral: 1.0
54-
safety_buffer_lateral: 0.7
60+
envelope_buffer_margin: 0.5
5561
safety_buffer_longitudinal: 0.0
5662
use_conservative_buffer_longitudinal: true
5763
trailer:
5864
execute_num: 1
5965
moving_speed_threshold: 1.0 # 3.6km/h
60-
moving_time_threshold: 1.0
66+
moving_time_threshold: 2.0
67+
lateral_margin:
68+
soft_margin: 0.9 # [m]
69+
hard_margin: 0.1 # [m]
70+
hard_margin_for_parked_vehicle: 0.1 # [m]
6171
max_expand_ratio: 0.0
62-
envelope_buffer_margin: 0.3
63-
avoid_margin_lateral: 1.0
64-
safety_buffer_lateral: 0.7
72+
envelope_buffer_margin: 0.5
6573
safety_buffer_longitudinal: 0.0
6674
use_conservative_buffer_longitudinal: true
6775
unknown:
6876
execute_num: 1
6977
moving_speed_threshold: 0.28 # 1.0km/h
7078
moving_time_threshold: 1.0
79+
lateral_margin:
80+
soft_margin: 0.7 # [m]
81+
hard_margin: -0.2 # [m]
82+
hard_margin_for_parked_vehicle: -0.2 # [m]
7183
max_expand_ratio: 0.0
72-
envelope_buffer_margin: 0.3
73-
avoid_margin_lateral: 1.0
74-
safety_buffer_lateral: 0.7
84+
envelope_buffer_margin: 0.1
7585
safety_buffer_longitudinal: 0.0
7686
use_conservative_buffer_longitudinal: true
7787
bicycle:
7888
execute_num: 1
7989
moving_speed_threshold: 0.28 # 1.0km/h
8090
moving_time_threshold: 1.0
91+
lateral_margin:
92+
soft_margin: 0.7 # [m]
93+
hard_margin: 0.5 # [m]
94+
hard_margin_for_parked_vehicle: 0.5 # [m]
8195
max_expand_ratio: 0.0
82-
envelope_buffer_margin: 0.8
83-
avoid_margin_lateral: 1.0
84-
safety_buffer_lateral: 1.0
96+
envelope_buffer_margin: 0.5
8597
safety_buffer_longitudinal: 1.0
8698
use_conservative_buffer_longitudinal: true
8799
motorcycle:
88100
execute_num: 1
89101
moving_speed_threshold: 1.0 # 3.6km/h
90102
moving_time_threshold: 1.0
103+
lateral_margin:
104+
soft_margin: 0.7 # [m]
105+
hard_margin: 0.3 # [m]
106+
hard_margin_for_parked_vehicle: 0.3 # [m]
91107
max_expand_ratio: 0.0
92-
envelope_buffer_margin: 0.8
93-
avoid_margin_lateral: 1.0
94-
safety_buffer_lateral: 1.0
108+
envelope_buffer_margin: 0.5
95109
safety_buffer_longitudinal: 1.0
96110
use_conservative_buffer_longitudinal: true
97111
pedestrian:
98112
execute_num: 1
99113
moving_speed_threshold: 0.28 # 1.0km/h
100114
moving_time_threshold: 1.0
115+
lateral_margin:
116+
soft_margin: 0.7 # [m]
117+
hard_margin: 0.5 # [m]
118+
hard_margin_for_parked_vehicle: 0.5 # [m]
101119
max_expand_ratio: 0.0
102-
envelope_buffer_margin: 0.8
103-
avoid_margin_lateral: 1.0
104-
safety_buffer_lateral: 1.0
120+
envelope_buffer_margin: 0.5
105121
safety_buffer_longitudinal: 1.0
106122
use_conservative_buffer_longitudinal: true
107123
lower_distance_for_polygon_expansion: 30.0 # [m]

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -69,9 +69,11 @@ struct ObjectParameter
6969

7070
double envelope_buffer_margin{0.0};
7171

72-
double avoid_margin_lateral{1.0};
72+
double lateral_soft_margin{1.0};
7373

74-
double safety_buffer_lateral{1.0};
74+
double lateral_hard_margin{1.0};
75+
76+
double lateral_hard_margin_for_parked_vehicle{1.0};
7577

7678
double safety_buffer_longitudinal{0.0};
7779

@@ -415,6 +417,9 @@ struct ObjectData // avoidance target
415417
// is within intersection area
416418
bool is_within_intersection{false};
417419

420+
// is parked vehicle on road shoulder
421+
bool is_parked{false};
422+
418423
// object direction.
419424
Direction direction{Direction::NONE};
420425

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
7272
param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
7373
param.envelope_buffer_margin =
7474
getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
75-
param.avoid_margin_lateral =
76-
getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
77-
param.safety_buffer_lateral =
78-
getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
75+
param.lateral_soft_margin =
76+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
77+
param.lateral_hard_margin =
78+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
79+
param.lateral_hard_margin_for_parked_vehicle =
80+
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
7981
param.safety_buffer_longitudinal =
8082
getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
8183
param.use_conservative_buffer_longitudinal =

‎planning/behavior_path_avoidance_module/src/debug.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
176176
marker.id = uuidToInt32(object.object.object_id);
177177
marker.pose.position.z += 2.0;
178178
std::ostringstream string_stream;
179-
string_stream << object.reason;
179+
string_stream << object.reason << (object.is_parked ? "(PARKED)" : "");
180180
marker.text = string_stream.str();
181181
marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999);
182182
marker.scale = createMarkerScale(0.6, 0.6, 0.6);

‎planning/behavior_path_avoidance_module/src/manager.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
6060
updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
6161
updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);
6262
updateParam<double>(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin);
63-
updateParam<double>(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral);
64-
updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
63+
updateParam<double>(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin);
64+
updateParam<double>(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin);
65+
updateParam<double>(
66+
parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle",
67+
config.lateral_hard_margin_for_parked_vehicle);
6568
updateParam<double>(
6669
parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
6770
updateParam<bool>(

‎planning/behavior_path_avoidance_module/src/scene.cpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -635,8 +635,8 @@ void AvoidanceModule::fillDebugData(
635635
: 0.0;
636636
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
637637

638-
const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
639-
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
638+
const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
639+
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
640640

641641
const auto variable = helper_->getSharpAvoidanceDistance(
642642
helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
@@ -1423,8 +1423,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
14231423
const auto object_type = utils::getHighestProbLabel(object.object.classification);
14241424
const auto object_parameter = parameters_->object_parameters.at(object_type);
14251425

1426-
const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
1427-
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
1426+
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
1427+
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
14281428
const auto variable = helper_->getMinAvoidanceDistance(
14291429
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
14301430
const auto & additional_buffer_longitudinal =
@@ -1653,8 +1653,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
16531653
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
16541654
const auto object_type = utils::getHighestProbLabel(object.object.classification);
16551655
const auto object_parameter = parameters_->object_parameters.at(object_type);
1656-
const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
1657-
object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
1656+
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
1657+
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
16581658
const auto shift_length =
16591659
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);
16601660

‎planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -234,9 +234,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
234234

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

237+
const auto lateral_hard_margin = object.is_parked
238+
? object_parameter.lateral_hard_margin_for_parked_vehicle
239+
: object_parameter.lateral_hard_margin;
237240
const auto infeasible =
238241
std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
239-
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
242+
0.5 * data_->parameters.vehicle_width + lateral_hard_margin;
240243
if (infeasible) {
241244
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
242245
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;

0 commit comments

Comments
 (0)