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

fix(avoidance): cherry pick PRs for avoidance module #1195

Merged
merged 4 commits into from
Mar 19, 2024
Merged

fix(avoidance): cherry pick PRs for avoidance module #1195

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2f54b41
feat(avoidance): change lateral margin based on if it's parked vehicl…
satoshi-ota Mar 7, 2024
f060da6
fix(drivable_area_expansion): fix invalid access (#6566)
satoshi-ota Mar 7, 2024
dc15ad5
fix(avoidance): ignore objects are in intersection when the ego stops…
satoshi-ota Mar 12, 2024
2bccaa1
fix(avoidance): the module ignored merging objects unexpectedly (#6601)
satoshi-ota Mar 15, 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
12 changes: 7 additions & 5 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 @@ 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 Expand Up @@ -130,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
}

{
const std::string ns = "avoidance.target_filtering.force_avoidance.";
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
p.enable_force_avoidance_for_stopped_vehicle =
getOrDeclareParameter<bool>(*node, ns + "enable");
p.threshold_time_force_avoidance_for_stopped_vehicle =
Expand Down
8 changes: 5 additions & 3 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 @@ -89,9 +89,11 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
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
74 changes: 45 additions & 29 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 @@ -30,81 +30,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 Expand Up @@ -140,7 +156,7 @@
backward_distance: 10.0 # [m]

# params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
force_avoidance:
avoidance_for_ambiguous_vehicle:
enable: false # [-]
time_threshold: 1.0 # [s]
distance_threshold: 1.0 # [m]
Expand Down
18 changes: 15 additions & 3 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 @@ -382,7 +384,8 @@ struct ObjectData // avoidance target
rclcpp::Time last_move;
double stop_time{0.0};

// store the information of the lanelet which the object's overhang is currently occupying
// It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to
// check whether the object is on the ego lane.
lanelet::ConstLanelet overhang_lanelet;

// the position at the detected moment
Expand Down Expand Up @@ -415,6 +418,15 @@ struct ObjectData // avoidance target
// is within intersection area
bool is_within_intersection{false};

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

// is driving on ego current lane
bool is_on_ego_lane{false};

// is ambiguous stopped vehicle.
bool is_ambiguous{false};

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

Expand Down
19 changes: 19 additions & 0 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "behavior_path_avoidance_module/data_structs.hpp"
#include "behavior_path_avoidance_module/utils.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"

#include <motion_utils/distance/distance.hpp>

Expand Down Expand Up @@ -134,6 +135,24 @@ class AvoidanceHelper
shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
}

double getFrontConstantDistance(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 & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
: 0.0;
return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
}

double getRearConstantDistance(const ObjectData & object) const
{
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
object.length;
}

double getEgoShift() const
{
validate();
Expand Down
12 changes: 7 additions & 5 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 Expand Up @@ -138,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
}

{
const std::string ns = "avoidance.target_filtering.force_avoidance.";
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
p.enable_force_avoidance_for_stopped_vehicle =
getOrDeclareParameter<bool>(*node, ns + "enable");
p.threshold_time_force_avoidance_for_stopped_vehicle =
Expand Down
Loading
Loading