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

feat(dynamic_avoidance): minimum object polygon longitudinal margin #4758

Merged
merged 2 commits into from
Aug 29, 2023
Merged

feat(dynamic_avoidance): minimum object polygon longitudinal margin #4758

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e17698b
feat(dynamic_avoidance): minimum object polygon longitudinal margin
takayuki5168 Aug 25, 2023
0da42d5
update config
takayuki5168 Aug 25, 2023
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
8 changes: 6 additions & 2 deletions planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
ros__parameters:
dynamic_avoidance:
common:
enable_debug_info: true
enable_debug_info: false
use_hatched_road_markings: true

# avoidance is performed for the object type with true
Expand Down Expand Up @@ -42,9 +42,13 @@
max_object_angle: 0.785

drivable_area_generation:
polygon_generation_method: "object_path_base" # choose "ego_path_base" and "object_path_base"
object_path_base:
min_longitudinal_polygon_margin: 3.0 # [m]

lat_offset_from_obstacle: 0.8 # [m]
max_lat_offset_to_avoid: 0.5 # [m]
max_time_for_object_lat_shift: 2.0 # [s]
max_time_for_object_lat_shift: 0.0 # [s]
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]

# for same directional object
Expand Down
1 change: 1 addition & 0 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ struct DynamicAvoidanceParameters

// drivable area generation
std::string polygon_generation_method{};
double min_obj_path_based_lon_polygon_margin{0.0};
double lat_offset_from_obstacle{0.0};
double max_lat_offset_to_avoid{0.0};
double max_time_for_lat_shift{0.0};
Expand Down
12 changes: 10 additions & 2 deletions ...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -949,11 +949,19 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
// calculate left and right bound
std::vector<geometry_msgs::msg::Point> obj_left_bound_points;
std::vector<geometry_msgs::msg::Point> obj_right_bound_points;
const double obj_path_length = motion_utils::calcArcLength(obj_path.path);
for (size_t i = 0; i < obj_path.path.size(); ++i) {
const double lon_offset = [&]() {
if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle;
if (i == 0)
return -object.shape.dimensions.x / 2.0 -
std::max(
parameters_->min_obj_path_based_lon_polygon_margin,
parameters_->lat_offset_from_obstacle);
if (i == obj_path.path.size() - 1)
return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle;
return object.shape.dimensions.x / 2.0 +
std::max(
parameters_->min_obj_path_based_lon_polygon_margin - obj_path_length,
parameters_->lat_offset_from_obstacle);
return 0.0;
}();

Expand Down
5 changes: 5 additions & 0 deletions planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
std::string ns = "dynamic_avoidance.drivable_area_generation.";
p.polygon_generation_method =
node->declare_parameter<std::string>(ns + "polygon_generation_method");
p.min_obj_path_based_lon_polygon_margin =
node->declare_parameter<double>(ns + "object_path_base.min_longitudinal_polygon_margin");
p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
p.max_lat_offset_to_avoid = node->declare_parameter<double>(ns + "max_lat_offset_to_avoid");
p.max_time_for_lat_shift =
Expand Down Expand Up @@ -182,6 +184,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams(

updateParam<std::string>(
parameters, ns + "polygon_generation_method", p->polygon_generation_method);
updateParam<double>(
parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
p->min_obj_path_based_lon_polygon_margin);
updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
updateParam<double>(
Expand Down