diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 98e2b21b8cc22..c26cfaa208277 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -15,6 +15,7 @@ min_obstacle_vel: 1.0 # [m/s] drivable_area_generation: - lat_offset_from_obstacle: 0.8 # [m] - time_to_avoid: 5.0 # [s] - max_lat_offset_to_avoid: 1.0 # [m] + lat_offset_from_obstacle: 2.0 # [m] + time_to_avoid_same_directional_object: 5.0 # [s] + time_to_avoid_opposite_directional_object: 6.0 # [s] + max_lat_offset_to_avoid: 2.0 # [m] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md index c4e458311b093..b428eadf3e7fd 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md @@ -11,12 +11,13 @@ Then the motion planner, in detail obstacle_avoidance_planner, will generate an ### Parameters -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ | -| target_object.car | [-] | bool | The flag whether to avoid cars or not | true | -| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true | -| ... | [-] | bool | ... | ... | -| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 | -| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 | -| drivable_area_generation.time_to_avoid | [s] | double | Elapsed time for avoiding an obstacle | 5.0 | -| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------------- | :---- | :----- | :---------------------------------------------------------- | :------------ | +| target_object.car | [-] | bool | The flag whether to avoid cars or not | true | +| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true | +| ... | [-] | bool | ... | ... | +| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 | +| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 | +| drivable_area_generation.time_to_avoid_same_directional_object | [s] | double | Elapsed time for avoiding the same directional obstacle | 5.0 | +| drivable_area_generation.time_to_avoid_opposite_directional_object | [s] | double | Elapsed time for avoiding the opposite directional obstacle | 6.0 | +| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 50a2d2ce5aa45..744be5348111d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -50,7 +50,8 @@ struct DynamicAvoidanceParameters // drivable area generation double lat_offset_from_obstacle{0.0}; - double time_to_avoid{0.0}; + double time_to_avoid_same_directional_object{0.0}; + double time_to_avoid_opposite_directional_object{0.0}; double max_lat_offset_to_avoid{0.0}; }; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 4f66e46c8d0b1..e0df52faba472 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -676,7 +676,10 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; p.lat_offset_from_obstacle = declare_parameter(ns + "lat_offset_from_obstacle"); - p.time_to_avoid = declare_parameter(ns + "time_to_avoid"); + p.time_to_avoid_same_directional_object = + declare_parameter(ns + "time_to_avoid_same_directional_object"); + p.time_to_avoid_opposite_directional_object = + declare_parameter(ns + "time_to_avoid_opposite_directional_object"); p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid"); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index d745b40d4bf39..17e91769fe40a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -307,26 +307,28 @@ std::optional DynamicAvoidanceModule::calcDynam }(); // calculate bound start and end index - const double length_to_avoid = object.path_projected_vel * parameters_->time_to_avoid; - [[maybe_unused]] const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + const double length_to_avoid = [&]() { + if (0.0 <= object.path_projected_vel) { + return object.path_projected_vel * parameters_->time_to_avoid_same_directional_object; + } + return object.path_projected_vel * parameters_->time_to_avoid_opposite_directional_object; + }(); + const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( obj_seg_idx, min_obj_lon_offset + (length_to_avoid < 0 ? length_to_avoid : 0.0), path_for_bound.points); const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, max_obj_lon_offset + (0 < length_to_avoid ? length_to_avoid : 0.0), + obj_seg_idx, max_obj_lon_offset + (0 <= length_to_avoid ? length_to_avoid : 0.0), path_for_bound.points); - const size_t lon_bound_start_idx = obj_seg_idx; + if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { + // NOTE: The obstacle is longitudinally out of the ego's trajectory. + return std::nullopt; + } + const size_t lon_bound_start_idx = + lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() : static_cast(path_for_bound.points.size() - 1); - // TODO(murooka) insertTargetPoint does not work well with a negative offset for now. - // When a offset is negative, the bound will be weird. - if ( - lon_bound_start_idx == 0 && - lon_bound_end_idx == static_cast(path_for_bound.points.size() - 1)) { - return std::nullopt; - } - // calculate bound min and max lateral offset const double min_bound_lat_offset = [&]() { const double raw_min_bound_lat_offset =