Skip to content

Commit

Permalink
feat(avoidance): configurable object type for safety check (#5699)
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 authored Nov 28, 2023
1 parent e3eb285 commit 5d9f3ac
Show file tree
Hide file tree
Showing 6 changed files with 107 additions and 26 deletions.
28 changes: 20 additions & 8 deletions planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
# avoidance is performed for the object type with true
target_object:
car:
is_target: true # [-]
execute_num: 1 # [-]
moving_speed_threshold: 1.0 # [m/s]
moving_time_threshold: 1.0 # [s]
Expand All @@ -38,7 +37,6 @@
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:
is_target: true
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -49,7 +47,6 @@
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bus:
is_target: true
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -60,7 +57,6 @@
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
trailer:
is_target: true
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -71,7 +67,6 @@
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
unknown:
is_target: true
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -82,7 +77,6 @@
safety_buffer_longitudinal: 0.0
use_conservative_buffer_longitudinal: true
bicycle:
is_target: true
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -93,7 +87,6 @@
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
motorcycle:
is_target: true
execute_num: 1
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -104,7 +97,6 @@
safety_buffer_longitudinal: 1.0
use_conservative_buffer_longitudinal: true
pedestrian:
is_target: true
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -119,6 +111,16 @@

# For target object filtering
target_filtering:
# avoidance target type
target_type:
car: true # [-]
truck: true # [-]
bus: true # [-]
trailer: true # [-]
unknown: true # [-]
bicycle: true # [-]
motorcycle: true # [-]
pedestrian: true # [-]
# detection range
object_check_goal_distance: 20.0 # [m]
# filtering parking objects
Expand Down Expand Up @@ -152,6 +154,16 @@

# For safety check
safety_check:
# safety check target type
target_type:
car: true # [-]
truck: true # [-]
bus: true # [-]
trailer: true # [-]
unknown: false # [-]
bicycle: true # [-]
motorcycle: true # [-]
pedestrian: true # [-]
# safety check configuration
enable: true # [-]
check_current_lane: false # [-]
Expand Down
21 changes: 13 additions & 8 deletions planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
# avoidance is performed for the object type with true
target_object:
car:
is_target: true # [-]
execute_num: 2 # [-]
moving_speed_threshold: 1.0 # [m/s]
moving_time_threshold: 1.0 # [s]
Expand All @@ -16,7 +15,6 @@
avoid_margin_lateral: 1.0 # [m]
safety_buffer_lateral: 0.7 # [m]
truck:
is_target: true
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -25,7 +23,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
bus:
is_target: true
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -34,7 +31,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
trailer:
is_target: true
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -43,7 +39,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.7
unknown:
is_target: true
execute_num: 1
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -52,7 +47,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 0.0
bicycle:
is_target: true
execute_num: 2
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -61,7 +55,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
motorcycle:
is_target: true
execute_num: 2
moving_speed_threshold: 1.0 # 3.6km/h
moving_time_threshold: 1.0
Expand All @@ -70,7 +63,6 @@
avoid_margin_lateral: 1.0
safety_buffer_lateral: 1.0
pedestrian:
is_target: true
execute_num: 2
moving_speed_threshold: 0.28 # 1.0km/h
moving_time_threshold: 1.0
Expand All @@ -80,3 +72,16 @@
safety_buffer_lateral: 1.0
lower_distance_for_polygon_expansion: 0.0 # [m]
upper_distance_for_polygon_expansion: 1.0 # [m]

# For target object filtering
target_filtering:
# avoidance target type
target_type:
car: true # [-]
truck: true # [-]
bus: true # [-]
trailer: true # [-]
unknown: true # [-]
bicycle: false # [-]
motorcycle: false # [-]
pedestrian: false # [-]
4 changes: 3 additions & 1 deletion ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;

struct ObjectParameter
{
bool is_target{false};
bool is_avoidance_target{false};

bool is_safety_check_target{false};

size_t execute_num{1};

Expand Down
36 changes: 34 additions & 2 deletions planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
{
const auto get_object_param = [&](std::string && ns) {
ObjectParameter param{};
param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
param.moving_speed_threshold =
getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
Expand Down Expand Up @@ -105,7 +104,24 @@ AvoidanceModuleManager::AvoidanceModuleManager(

// target filtering
{
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
if (p.object_parameters.count(object_type) == 0) {
return;
}
p.object_parameters.at(object_type).is_avoidance_target =
getOrDeclareParameter<bool>(*node, ns);
};

std::string ns = "avoidance.target_filtering.";
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");

p.object_check_goal_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
p.threshold_distance_object_is_on_center =
Expand Down Expand Up @@ -147,7 +163,24 @@ AvoidanceModuleManager::AvoidanceModuleManager(

// safety check general params
{
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
if (p.object_parameters.count(object_type) == 0) {
return;
}
p.object_parameters.at(object_type).is_safety_check_target =
getOrDeclareParameter<bool>(*node, ns);
};

std::string ns = "avoidance.safety_check.";
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");

p.enable_safety_check = getOrDeclareParameter<bool>(*node, ns + "enable");
p.check_current_lane = getOrDeclareParameter<bool>(*node, ns + "check_current_lane");
p.check_shift_side_lane = getOrDeclareParameter<bool>(*node, ns + "check_shift_side_lane");
Expand Down Expand Up @@ -346,7 +379,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
const auto update_object_param = [&p, &parameters](
const auto & semantic, const std::string & ns) {
auto & config = p->object_parameters.at(semantic);
updateParam<bool>(parameters, ns + "is_target", config.is_target);
updateParam<double>(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold);
updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);
Expand Down
18 changes: 17 additions & 1 deletion planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
{
const auto get_object_param = [&](std::string && ns) {
ObjectParameter param{};
param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
param.moving_speed_threshold =
getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
Expand Down Expand Up @@ -321,7 +320,24 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(

// target filtering
{
const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
if (p.object_parameters.count(object_type) == 0) {
return;
}
p.object_parameters.at(object_type).is_avoidance_target =
getOrDeclareParameter<bool>(*node, ns);
};

std::string ns = "avoidance.target_filtering.";
set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");

p.object_check_goal_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
p.threshold_distance_object_is_on_center =
Expand Down
26 changes: 20 additions & 6 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,7 +235,7 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)

namespace filtering_utils
{
bool isTargetObjectType(
bool isAvoidanceTargetObjectType(
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto object_type = utils::getHighestProbLabel(object.classification);
Expand All @@ -244,7 +244,19 @@ bool isTargetObjectType(
return false;
}

return parameters->object_parameters.at(object_type).is_target;
return parameters->object_parameters.at(object_type).is_avoidance_target;
}

bool isSafetyCheckTargetObjectType(
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto object_type = utils::getHighestProbLabel(object.classification);

if (parameters->object_parameters.count(object_type) == 0) {
return false;
}

return parameters->object_parameters.at(object_type).is_safety_check_target;
}

bool isVehicleTypeObject(const ObjectData & object)
Expand Down Expand Up @@ -500,7 +512,7 @@ bool isSatisfiedWithCommonCondition(
const std::shared_ptr<AvoidanceParameters> & parameters)
{
// Step1. filtered by target object type.
if (!isTargetObjectType(object.object, parameters)) {
if (!isAvoidanceTargetObjectType(object.object, parameters)) {
object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE;
return false;
}
Expand Down Expand Up @@ -1705,10 +1717,12 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
});
};

const auto to_predicted_objects = [&p](const auto & objects) {
const auto to_predicted_objects = [&p, &parameters](const auto & objects) {
PredictedObjects ret{};
std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) {
ret.objects.push_back(object.object);
std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
ret.objects.push_back(object.object);
}
});
return ret;
};
Expand Down

0 comments on commit 5d9f3ac

Please sign in to comment.