Skip to content

Commit 5d9f3ac

Browse files
satoshi-otasatoshi-ota
authored
feat(avoidance): configurable object type for safety check (#5699)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent e3eb285 commit 5d9f3ac

File tree

6 files changed

+107
-26
lines changed

6 files changed

+107
-26
lines changed

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

+20-8
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,6 @@
2727
# avoidance is performed for the object type with true
2828
target_object:
2929
car:
30-
is_target: true # [-]
3130
execute_num: 1 # [-]
3231
moving_speed_threshold: 1.0 # [m/s]
3332
moving_time_threshold: 1.0 # [s]
@@ -38,7 +37,6 @@
3837
safety_buffer_longitudinal: 0.0 # [m]
3938
use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
4039
truck:
41-
is_target: true
4240
execute_num: 1
4341
moving_speed_threshold: 1.0 # 3.6km/h
4442
moving_time_threshold: 1.0
@@ -49,7 +47,6 @@
4947
safety_buffer_longitudinal: 0.0
5048
use_conservative_buffer_longitudinal: true
5149
bus:
52-
is_target: true
5350
execute_num: 1
5451
moving_speed_threshold: 1.0 # 3.6km/h
5552
moving_time_threshold: 1.0
@@ -60,7 +57,6 @@
6057
safety_buffer_longitudinal: 0.0
6158
use_conservative_buffer_longitudinal: true
6259
trailer:
63-
is_target: true
6460
execute_num: 1
6561
moving_speed_threshold: 1.0 # 3.6km/h
6662
moving_time_threshold: 1.0
@@ -71,7 +67,6 @@
7167
safety_buffer_longitudinal: 0.0
7268
use_conservative_buffer_longitudinal: true
7369
unknown:
74-
is_target: true
7570
execute_num: 1
7671
moving_speed_threshold: 0.28 # 1.0km/h
7772
moving_time_threshold: 1.0
@@ -82,7 +77,6 @@
8277
safety_buffer_longitudinal: 0.0
8378
use_conservative_buffer_longitudinal: true
8479
bicycle:
85-
is_target: true
8680
execute_num: 1
8781
moving_speed_threshold: 0.28 # 1.0km/h
8882
moving_time_threshold: 1.0
@@ -93,7 +87,6 @@
9387
safety_buffer_longitudinal: 1.0
9488
use_conservative_buffer_longitudinal: true
9589
motorcycle:
96-
is_target: true
9790
execute_num: 1
9891
moving_speed_threshold: 1.0 # 3.6km/h
9992
moving_time_threshold: 1.0
@@ -104,7 +97,6 @@
10497
safety_buffer_longitudinal: 1.0
10598
use_conservative_buffer_longitudinal: true
10699
pedestrian:
107-
is_target: true
108100
execute_num: 1
109101
moving_speed_threshold: 0.28 # 1.0km/h
110102
moving_time_threshold: 1.0
@@ -119,6 +111,16 @@
119111

120112
# For target object filtering
121113
target_filtering:
114+
# avoidance target type
115+
target_type:
116+
car: true # [-]
117+
truck: true # [-]
118+
bus: true # [-]
119+
trailer: true # [-]
120+
unknown: true # [-]
121+
bicycle: true # [-]
122+
motorcycle: true # [-]
123+
pedestrian: true # [-]
122124
# detection range
123125
object_check_goal_distance: 20.0 # [m]
124126
# filtering parking objects
@@ -152,6 +154,16 @@
152154

153155
# For safety check
154156
safety_check:
157+
# safety check target type
158+
target_type:
159+
car: true # [-]
160+
truck: true # [-]
161+
bus: true # [-]
162+
trailer: true # [-]
163+
unknown: false # [-]
164+
bicycle: true # [-]
165+
motorcycle: true # [-]
166+
pedestrian: true # [-]
155167
# safety check configuration
156168
enable: true # [-]
157169
check_current_lane: false # [-]

‎planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml

+13-8
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,6 @@
77
# avoidance is performed for the object type with true
88
target_object:
99
car:
10-
is_target: true # [-]
1110
execute_num: 2 # [-]
1211
moving_speed_threshold: 1.0 # [m/s]
1312
moving_time_threshold: 1.0 # [s]
@@ -16,7 +15,6 @@
1615
avoid_margin_lateral: 1.0 # [m]
1716
safety_buffer_lateral: 0.7 # [m]
1817
truck:
19-
is_target: true
2018
execute_num: 2
2119
moving_speed_threshold: 1.0 # 3.6km/h
2220
moving_time_threshold: 1.0
@@ -25,7 +23,6 @@
2523
avoid_margin_lateral: 1.0
2624
safety_buffer_lateral: 0.7
2725
bus:
28-
is_target: true
2926
execute_num: 2
3027
moving_speed_threshold: 1.0 # 3.6km/h
3128
moving_time_threshold: 1.0
@@ -34,7 +31,6 @@
3431
avoid_margin_lateral: 1.0
3532
safety_buffer_lateral: 0.7
3633
trailer:
37-
is_target: true
3834
execute_num: 2
3935
moving_speed_threshold: 1.0 # 3.6km/h
4036
moving_time_threshold: 1.0
@@ -43,7 +39,6 @@
4339
avoid_margin_lateral: 1.0
4440
safety_buffer_lateral: 0.7
4541
unknown:
46-
is_target: true
4742
execute_num: 1
4843
moving_speed_threshold: 0.28 # 1.0km/h
4944
moving_time_threshold: 1.0
@@ -52,7 +47,6 @@
5247
avoid_margin_lateral: 1.0
5348
safety_buffer_lateral: 0.0
5449
bicycle:
55-
is_target: true
5650
execute_num: 2
5751
moving_speed_threshold: 0.28 # 1.0km/h
5852
moving_time_threshold: 1.0
@@ -61,7 +55,6 @@
6155
avoid_margin_lateral: 1.0
6256
safety_buffer_lateral: 1.0
6357
motorcycle:
64-
is_target: true
6558
execute_num: 2
6659
moving_speed_threshold: 1.0 # 3.6km/h
6760
moving_time_threshold: 1.0
@@ -70,7 +63,6 @@
7063
avoid_margin_lateral: 1.0
7164
safety_buffer_lateral: 1.0
7265
pedestrian:
73-
is_target: true
7466
execute_num: 2
7567
moving_speed_threshold: 0.28 # 1.0km/h
7668
moving_time_threshold: 1.0
@@ -80,3 +72,16 @@
8072
safety_buffer_lateral: 1.0
8173
lower_distance_for_polygon_expansion: 0.0 # [m]
8274
upper_distance_for_polygon_expansion: 1.0 # [m]
75+
76+
# For target object filtering
77+
target_filtering:
78+
# avoidance target type
79+
target_type:
80+
car: true # [-]
81+
truck: true # [-]
82+
bus: true # [-]
83+
trailer: true # [-]
84+
unknown: true # [-]
85+
bicycle: false # [-]
86+
motorcycle: false # [-]
87+
pedestrian: false # [-]

‎planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
5454

5555
struct ObjectParameter
5656
{
57-
bool is_target{false};
57+
bool is_avoidance_target{false};
58+
59+
bool is_safety_check_target{false};
5860

5961
size_t execute_num{1};
6062

‎planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

+34-2
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
6565
{
6666
const auto get_object_param = [&](std::string && ns) {
6767
ObjectParameter param{};
68-
param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
6968
param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
7069
param.moving_speed_threshold =
7170
getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
@@ -105,7 +104,24 @@ AvoidanceModuleManager::AvoidanceModuleManager(
105104

106105
// target filtering
107106
{
107+
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
108+
if (p.object_parameters.count(object_type) == 0) {
109+
return;
110+
}
111+
p.object_parameters.at(object_type).is_avoidance_target =
112+
getOrDeclareParameter<bool>(*node, ns);
113+
};
114+
108115
std::string ns = "avoidance.target_filtering.";
116+
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
117+
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
118+
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
119+
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
120+
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
121+
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
122+
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
123+
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
124+
109125
p.object_check_goal_distance =
110126
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
111127
p.threshold_distance_object_is_on_center =
@@ -147,7 +163,24 @@ AvoidanceModuleManager::AvoidanceModuleManager(
147163

148164
// safety check general params
149165
{
166+
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
167+
if (p.object_parameters.count(object_type) == 0) {
168+
return;
169+
}
170+
p.object_parameters.at(object_type).is_safety_check_target =
171+
getOrDeclareParameter<bool>(*node, ns);
172+
};
173+
150174
std::string ns = "avoidance.safety_check.";
175+
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
176+
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
177+
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
178+
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
179+
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
180+
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
181+
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
182+
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
183+
151184
p.enable_safety_check = getOrDeclareParameter<bool>(*node, ns + "enable");
152185
p.check_current_lane = getOrDeclareParameter<bool>(*node, ns + "check_current_lane");
153186
p.check_shift_side_lane = getOrDeclareParameter<bool>(*node, ns + "check_shift_side_lane");
@@ -346,7 +379,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
346379
const auto update_object_param = [&p, &parameters](
347380
const auto & semantic, const std::string & ns) {
348381
auto & config = p->object_parameters.at(semantic);
349-
updateParam<bool>(parameters, ns + "is_target", config.is_target);
350382
updateParam<double>(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold);
351383
updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
352384
updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);

‎planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

+17-1
Original file line numberDiff line numberDiff line change
@@ -285,7 +285,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
285285
{
286286
const auto get_object_param = [&](std::string && ns) {
287287
ObjectParameter param{};
288-
param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
289288
param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
290289
param.moving_speed_threshold =
291290
getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
@@ -321,7 +320,24 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
321320

322321
// target filtering
323322
{
323+
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
324+
if (p.object_parameters.count(object_type) == 0) {
325+
return;
326+
}
327+
p.object_parameters.at(object_type).is_avoidance_target =
328+
getOrDeclareParameter<bool>(*node, ns);
329+
};
330+
324331
std::string ns = "avoidance.target_filtering.";
332+
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
333+
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
334+
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
335+
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
336+
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
337+
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
338+
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
339+
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
340+
325341
p.object_check_goal_distance =
326342
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
327343
p.threshold_distance_object_is_on_center =

‎planning/behavior_path_planner/src/utils/avoidance/utils.cpp

+20-6
Original file line numberDiff line numberDiff line change
@@ -235,7 +235,7 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
235235

236236
namespace filtering_utils
237237
{
238-
bool isTargetObjectType(
238+
bool isAvoidanceTargetObjectType(
239239
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
240240
{
241241
const auto object_type = utils::getHighestProbLabel(object.classification);
@@ -244,7 +244,19 @@ bool isTargetObjectType(
244244
return false;
245245
}
246246

247-
return parameters->object_parameters.at(object_type).is_target;
247+
return parameters->object_parameters.at(object_type).is_avoidance_target;
248+
}
249+
250+
bool isSafetyCheckTargetObjectType(
251+
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
252+
{
253+
const auto object_type = utils::getHighestProbLabel(object.classification);
254+
255+
if (parameters->object_parameters.count(object_type) == 0) {
256+
return false;
257+
}
258+
259+
return parameters->object_parameters.at(object_type).is_safety_check_target;
248260
}
249261

250262
bool isVehicleTypeObject(const ObjectData & object)
@@ -500,7 +512,7 @@ bool isSatisfiedWithCommonCondition(
500512
const std::shared_ptr<AvoidanceParameters> & parameters)
501513
{
502514
// Step1. filtered by target object type.
503-
if (!isTargetObjectType(object.object, parameters)) {
515+
if (!isAvoidanceTargetObjectType(object.object, parameters)) {
504516
object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE;
505517
return false;
506518
}
@@ -1705,10 +1717,12 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
17051717
});
17061718
};
17071719

1708-
const auto to_predicted_objects = [&p](const auto & objects) {
1720+
const auto to_predicted_objects = [&p, &parameters](const auto & objects) {
17091721
PredictedObjects ret{};
1710-
std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) {
1711-
ret.objects.push_back(object.object);
1722+
std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
1723+
if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
1724+
ret.objects.push_back(object.object);
1725+
}
17121726
});
17131727
return ret;
17141728
};

0 commit comments

Comments
 (0)