Skip to content

Commit 01fdac0

Browse files
authored
feat(goal_planner): consider moving objects when deciding path (#6178)
feat(goal_planner): consider move objects when deciding path Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 7022620 commit 01fdac0

File tree

2 files changed

+38
-17
lines changed

2 files changed

+38
-17
lines changed

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -419,7 +419,7 @@ class GoalPlannerModule : public SceneModuleInterface
419419
bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
420420
bool checkObjectsCollision(
421421
const PathWithLaneId & path, const double collision_check_margin,
422-
const bool update_debug_data = false) const;
422+
const bool extract_static_objects, const bool update_debug_data = false) const;
423423

424424
// goal seach
425425
Pose calcRefinedGoal(const Pose & goal_pose) const;

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

+37-16
Original file line numberDiff line numberDiff line change
@@ -624,7 +624,9 @@ bool GoalPlannerModule::canReturnToLaneParking()
624624

625625
if (
626626
parameters_->use_object_recognition &&
627-
checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) {
627+
checkObjectsCollision(
628+
path, parameters_->object_recognition_collision_check_margin,
629+
/*extract_static_objects=*/false)) {
628630
return false;
629631
}
630632

@@ -711,7 +713,9 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
711713
const auto resampled_path =
712714
utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
713715
for (const auto & scale_factor : scale_factors) {
714-
if (!checkObjectsCollision(resampled_path, margin * scale_factor)) {
716+
if (!checkObjectsCollision(
717+
resampled_path, margin * scale_factor,
718+
/*extract_static_objects=*/true)) {
715719
return margin * scale_factor;
716720
}
717721
}
@@ -771,8 +775,10 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
771775

772776
const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
773777
if (
774-
parameters_->use_object_recognition &&
775-
checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) {
778+
parameters_->use_object_recognition && checkObjectsCollision(
779+
resampled_path, collision_check_margin,
780+
/*extract_static_objects=*/true,
781+
/*update_debug_data=*/true)) {
776782
continue;
777783
}
778784

@@ -914,6 +920,7 @@ bool GoalPlannerModule::hasNotDecidedPath() const
914920
DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
915921
{
916922
const auto & prev_status = prev_data_.deciding_path_status;
923+
const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check;
917924

918925
// Once this function returns true, it will continue to return true thereafter
919926
if (prev_status.first == DecidingPathStatus::DECIDED) {
@@ -927,8 +934,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
927934

928935
// If it is dangerous against dynamic objects before approval, do not determine the path.
929936
// This eliminates a unsafe path to be approved
930-
if (
931-
parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) {
937+
if (enable_safety_check && !isSafePath().first && !isActivated()) {
932938
RCLCPP_DEBUG(
933939
getLogger(),
934940
"[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before "
@@ -957,13 +963,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
957963
const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5);
958964
const double margin =
959965
parameters_->object_recognition_collision_check_margin * hysteresis_factor;
960-
if (checkObjectsCollision(parking_path, margin)) {
966+
if (checkObjectsCollision(parking_path, margin, /*extract_static_objects=*/false)) {
961967
RCLCPP_DEBUG(
962968
getLogger(),
963969
"[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects");
964970
return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
965971
}
966972

973+
if (enable_safety_check && !isSafePath().first) {
974+
RCLCPP_DEBUG(
975+
getLogger(),
976+
"[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects");
977+
return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
978+
}
979+
967980
// if enough time has passed since deciding status starts, transition to DECIDED
968981
constexpr double check_collision_duration = 1.0;
969982
const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds();
@@ -1406,7 +1419,7 @@ bool GoalPlannerModule::isStuck()
14061419
parameters_->use_object_recognition &&
14071420
checkObjectsCollision(
14081421
thread_safe_data_.get_pull_over_path()->getCurrentPath(),
1409-
parameters_->object_recognition_collision_check_margin)) {
1422+
/*extract_static_objects=*/false, parameters_->object_recognition_collision_check_margin)) {
14101423
return true;
14111424
}
14121425

@@ -1519,16 +1532,24 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path)
15191532

15201533
bool GoalPlannerModule::checkObjectsCollision(
15211534
const PathWithLaneId & path, const double collision_check_margin,
1522-
const bool update_debug_data) const
1523-
{
1524-
const auto pull_over_lane_stop_objects =
1525-
goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
1526-
*(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
1527-
parameters_->forward_goal_search_length, parameters_->detection_bound_offset,
1528-
*(planner_data_->dynamic_object), parameters_->th_moving_object_velocity);
1535+
const bool extract_static_objects, const bool update_debug_data) const
1536+
{
1537+
const auto target_objects = std::invoke([&]() {
1538+
const auto & p = parameters_;
1539+
const auto & rh = *(planner_data_->route_handler);
1540+
const auto objects = *(planner_data_->dynamic_object);
1541+
if (extract_static_objects) {
1542+
return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
1543+
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
1544+
p->detection_bound_offset, objects, p->th_moving_object_velocity);
1545+
}
1546+
return goal_planner_utils::extractObjectsInExpandedPullOverLanes(
1547+
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
1548+
p->detection_bound_offset, objects);
1549+
});
15291550

15301551
std::vector<Polygon2d> obj_polygons;
1531-
for (const auto & object : pull_over_lane_stop_objects.objects) {
1552+
for (const auto & object : target_objects.objects) {
15321553
obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object));
15331554
}
15341555

0 commit comments

Comments
 (0)