Skip to content

Commit

Permalink
feat(avoidance): change lateral margin based on if it's parked vehicle (
Browse files Browse the repository at this point in the history 
#6520)

* feat(avoidance): check if it's parked vehicle

Signed-off-by: satoshi-ota <[email protected]>

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

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
Signed-off-by: Kotaro Yoshimoto <[email protected]>
  • Loading branch information
@satoshi-ota @HansRobo
satoshi-ota authored and HansRobo committed Mar 12, 2024
1 parent 717e369 commit c79d248
Show file tree
Hide file tree
Showing 11 changed files with 158 additions and 103 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
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
Original file line number Diff line number Diff line change
Expand Up @@ -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");
return param;
};

Expand Down
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
Expand Up @@ -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;

avoidance_helper_->setData(planner_data_);
const auto shift_length = avoidance_helper_->getShiftLength(
Expand Down Expand Up @@ -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;
Expand Down
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
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
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
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -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);
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
Original file line number Diff line number Diff line change
Expand Up @@ -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;

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 @@ 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;
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 @@ 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 +
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
Original file line number Diff line number Diff line change
Expand Up @@ -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
? 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;
if (infeasible) {
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
Expand Down
Loading

0 comments on commit c79d248

Please sign in to comment.