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(start_planner): cherry pick #1173

Merged
merged 1 commit into from
Mar 6, 2024
Merged

feat(start_planner): cherry pick #1173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
34 changes: 21 additions & 13 deletions planning/behavior_path_start_planner_module/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,19 +65,27 @@ PullOutPath --o PullOutPlannerBase

## General parameters for start_planner

| Name | Unit | Type | Description | Default value |
| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- |
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] |
| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
| Name | Unit | Type | Description | Default value |
| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- |
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

## Safety check with static obstacles

Expand Down
9 changes: 9 additions & 0 deletions planning/behavior_path_start_planner_module/config/start_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,15 @@
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
collision_check_margin_from_front_object: 5.0
th_moving_object_velocity: 1.0
object_types_to_check_for_path_generation:
check_car: true
check_truck: true
check_bus: true
check_trailer: true
check_bicycle: true
check_motorcycle: true
check_pedestrian: true
check_unknown: true
th_distance_to_middle_of_the_road: 0.5
center_line_path_interval: 1.0
# shift pull out
Expand Down
2 changes: 2 additions & 0 deletions ...rt_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ struct StartPlannerParameters
std::vector<double> collision_check_margins{};
double collision_check_margin_from_front_object{0.0};
double th_moving_object_velocity{0.0};
behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck
object_types_to_check_for_path_generation{};
double center_line_path_interval{0.0};

// shift pull out
Expand Down
46 changes: 46 additions & 0 deletions planning/behavior_path_start_planner_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,25 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.collision_check_margin_from_front_object =
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
{
const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
p.object_types_to_check_for_path_generation.check_car =
node->declare_parameter<bool>(ns + "check_car");
p.object_types_to_check_for_path_generation.check_truck =
node->declare_parameter<bool>(ns + "check_truck");
p.object_types_to_check_for_path_generation.check_bus =
node->declare_parameter<bool>(ns + "check_bus");
p.object_types_to_check_for_path_generation.check_trailer =
node->declare_parameter<bool>(ns + "check_trailer");
p.object_types_to_check_for_path_generation.check_unknown =
node->declare_parameter<bool>(ns + "check_unknown");
p.object_types_to_check_for_path_generation.check_bicycle =
node->declare_parameter<bool>(ns + "check_bicycle");
p.object_types_to_check_for_path_generation.check_motorcycle =
node->declare_parameter<bool>(ns + "check_motorcycle");
p.object_types_to_check_for_path_generation.check_pedestrian =
node->declare_parameter<bool>(ns + "check_pedestrian");
}
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
// shift pull out
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
Expand Down Expand Up @@ -356,6 +375,33 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, ns + "collision_check_margin_from_front_object",
p->collision_check_margin_from_front_object);
updateParam<double>(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity);
const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation.";
{
updateParam<bool>(
parameters, obj_types_ns + "check_car",
p->object_types_to_check_for_path_generation.check_car);
updateParam<bool>(
parameters, obj_types_ns + "check_truck",
p->object_types_to_check_for_path_generation.check_truck);
updateParam<bool>(
parameters, obj_types_ns + "check_bus",
p->object_types_to_check_for_path_generation.check_bus);
updateParam<bool>(
parameters, obj_types_ns + "check_trailer",
p->object_types_to_check_for_path_generation.check_trailer);
updateParam<bool>(
parameters, obj_types_ns + "check_unknown",
p->object_types_to_check_for_path_generation.check_unknown);
updateParam<bool>(
parameters, obj_types_ns + "check_bicycle",
p->object_types_to_check_for_path_generation.check_bicycle);
updateParam<bool>(
parameters, obj_types_ns + "check_motorcycle",
p->object_types_to_check_for_path_generation.check_motorcycle);
updateParam<bool>(
parameters, obj_types_ns + "check_pedestrian",
p->object_types_to_check_for_path_generation.check_pedestrian);
}
updateParam<double>(parameters, ns + "center_line_path_interval", p->center_line_path_interval);
updateParam<bool>(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out);
updateParam<double>(
Expand Down
10 changes: 7 additions & 3 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -625,9 +625,10 @@ bool StartPlannerModule::findPullOutPath(
// extract stop objects in pull out lane for collision check
const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
*dynamic_objects, parameters_->th_moving_object_velocity);
const auto [pull_out_lane_stop_objects, others] =
utils::path_safety_checker::separateObjectsByLanelets(
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets(
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
utils::path_safety_checker::filterObjectsByClass(
pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation);

// if start_pose_candidate is far from refined_start_pose, backward driving is necessary
const bool backward_is_unnecessary =
Expand Down Expand Up @@ -993,6 +994,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance,
object_check_backward_distance);

utils::path_safety_checker::filterObjectsByClass(
stop_objects_in_pull_out_lanes, parameters_->object_types_to_check_for_path_generation);

return stop_objects_in_pull_out_lanes;
}

Expand Down
Loading