Skip to content

Commit

Permalink
feat(start_planner): add object_types_to_check_for_path_generation (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#6370)

* add object_types_to_check_for_path_generation

Signed-off-by: kyoichi-sugahara <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
Signed-off-by: kaigohirao <[email protected]>
  • Loading branch information
@kyoichi-sugahara @kaigohirao
kyoichi-sugahara authored and kaigohirao committed Mar 22, 2024
1 parent 30d1bea commit b6c4db9
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 3 deletions.
7 changes: 7 additions & 0 deletions planning/behavior_path_start_planner_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -332,6 +332,13 @@ PullOutPath --o PullOutPlannerBase
| 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 |

### **Ego vehicle's velocity planning**
Expand Down
9 changes: 9 additions & 0 deletions planning/behavior_path_start_planner_module/config/start_planner.param.yaml
View file
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 ...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,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
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,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 @@ -358,6 +377,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
Original file line number Diff line number Diff line change
Expand Up @@ -668,9 +668,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 @@ -1036,6 +1037,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

0 comments on commit b6c4db9

Please sign in to comment.