From e8b05c23c6b4f74cde377d122a63b84a02478a77 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 27 Jan 2025 08:46:10 +0900 Subject: [PATCH] refactor(goal_planner): divide extract_dynamic_object/is_goal_in_lanes util function (#10019) Signed-off-by: Mamoru Sobue --- .../util.hpp | 9 +++ .../src/goal_planner_module.cpp | 64 +++--------------- .../src/util.cpp | 65 +++++++++++++++++++ 3 files changed, 82 insertions(+), 56 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f00d9d41622e9..7f375da295fb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -185,6 +185,15 @@ std::optional calcRefinedGoal( std::optional calcClosestPose( const lanelet::ConstLineString3d line, const Point & query_point); +autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( + const autoware_perception_msgs::msg::PredictedObjects & original_objects, + const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters, + const double vehicle_width); + +bool is_goal_reachable_on_path( + const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking); + } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index c7e32a47a59f9..0a92f6d99dfeb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -615,29 +615,11 @@ void GoalPlannerModule::updateData() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // extract static and dynamic objects in extraction polygon for path collision check - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); - const double vehicle_width = planner_data_->parameters.vehicle_width; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking_, p->detection_bound_offset, - p->margin_from_boundary + p->max_lateral_offset + vehicle_width); - if (objects_extraction_polygon.has_value()) { - debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); - } - PredictedObjects dynamic_target_objects{}; - for (const auto & object : objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } - } + const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( + *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_, + planner_data_->parameters.vehicle_width); const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, p->th_moving_object_velocity); + dynamic_target_objects, parameters_->th_moving_object_velocity); // update goal searcher and generate goal candidates if (goal_candidates_.empty()) { @@ -753,39 +735,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes or neighboring road lanes const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - const auto getNeighboringLane = - [&](const lanelet::ConstLanelet & lane) -> std::optional { - return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) - : route_handler->getRightLanelet(lane, false, true); - }; - lanelet::ConstLanelets goal_check_lanes = current_lanes; - for (const auto & lane : current_lanes) { - auto neighboring_lane = getNeighboringLane(lane); - while (neighboring_lane) { - goal_check_lanes.push_back(neighboring_lane.value()); - neighboring_lane = getNeighboringLane(neighboring_lane.value()); - } - } - const bool goal_is_in_current_segment_lanes = std::any_of( - goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { - return lanelet::utils::isInLanelet(goal_pose, lane); - }); - - // check that goal is in current neighbor shoulder lane - const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { - for (const auto & lane : current_lanes) { - const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane) - : route_handler->getRightShoulderLanelet(lane); - if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { - return true; - } - } - return false; - }); - - // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, - // because goal arc coordinates cannot be calculated. - if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) { + const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path( + current_lanes, *(planner_data_->route_handler), left_side_parking_); + if (!is_goal_reachable) { return false; } @@ -793,7 +745,7 @@ bool GoalPlannerModule::isExecutionRequested() const // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_segment_lanes; + return is_goal_reachable; } // if goal arc coordinates can be calculated, check if goal is in request_length diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index a2a64fc700676..fb39a34c9ac91 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -831,4 +831,69 @@ std::optional calcClosestPose( return closest_pose; } +autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( + const autoware_perception_msgs::msg::PredictedObjects & original_objects, + const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters, + const double vehicle_width) +{ + const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + route_handler, left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking, parameters.detection_bound_offset, + parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width); + + PredictedObjects dynamic_target_objects{}; + for (const auto & object : original_objects.objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); + } + } + return dynamic_target_objects; +} + +bool is_goal_reachable_on_path( + const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const Pose goal_pose = route_handler.getOriginalGoalPose(); + const auto getNeighboringLane = + [&](const lanelet::ConstLanelet & lane) -> std::optional { + return left_side_parking ? route_handler.getLeftLanelet(lane, false, true) + : route_handler.getRightLanelet(lane, false, true); + }; + lanelet::ConstLanelets goal_check_lanes = current_lanes; + for (const auto & lane : current_lanes) { + auto neighboring_lane = getNeighboringLane(lane); + while (neighboring_lane) { + goal_check_lanes.push_back(neighboring_lane.value()); + neighboring_lane = getNeighboringLane(neighboring_lane.value()); + } + } + const bool goal_is_in_current_segment_lanes = std::any_of( + goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { + return lanelet::utils::isInLanelet(goal_pose, lane); + }); + + // check that goal is in current neighbor shoulder lane + const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { + for (const auto & lane : current_lanes) { + const auto shoulder_lane = left_side_parking ? route_handler.getLeftShoulderLanelet(lane) + : route_handler.getRightShoulderLanelet(lane); + if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { + return true; + } + } + return false; + }); + + // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, + // because goal arc coordinates cannot be calculated. + return goal_is_in_current_segment_lanes || goal_is_in_current_shoulder_lanes; +} + } // namespace autoware::behavior_path_planner::goal_planner_utils