Skip to content

Commit e8b05c2

Browse files
authored
refactor(goal_planner): divide extract_dynamic_object/is_goal_in_lanes util function (autowarefoundation#10019)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 40aff9f commit e8b05c2

File tree

3 files changed

+82
-56
lines changed

3 files changed

+82
-56
lines changed

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

+9
Original file line numberDiff line numberDiff line change
@@ -185,6 +185,15 @@ std::optional<Pose> calcRefinedGoal(
185185
std::optional<Pose> calcClosestPose(
186186
const lanelet::ConstLineString3d line, const Point & query_point);
187187

188+
autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects(
189+
const autoware_perception_msgs::msg::PredictedObjects & original_objects,
190+
const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters,
191+
const double vehicle_width);
192+
193+
bool is_goal_reachable_on_path(
194+
const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler,
195+
const bool left_side_parking);
196+
188197
} // namespace autoware::behavior_path_planner::goal_planner_utils
189198

190199
#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+8-56
Original file line numberDiff line numberDiff line change
@@ -615,29 +615,11 @@ void GoalPlannerModule::updateData()
615615
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
616616

617617
// extract static and dynamic objects in extraction polygon for path collision check
618-
const auto & p = parameters_;
619-
const auto & rh = *(planner_data_->route_handler);
620-
const auto objects = *(planner_data_->dynamic_object);
621-
const double vehicle_width = planner_data_->parameters.vehicle_width;
622-
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
623-
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length);
624-
const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
625-
pull_over_lanes, left_side_parking_, p->detection_bound_offset,
626-
p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
627-
if (objects_extraction_polygon.has_value()) {
628-
debug_data_.objects_extraction_polygon = objects_extraction_polygon.value();
629-
}
630-
PredictedObjects dynamic_target_objects{};
631-
for (const auto & object : objects.objects) {
632-
const auto object_polygon = universe_utils::toPolygon2d(object);
633-
if (
634-
objects_extraction_polygon.has_value() &&
635-
boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
636-
dynamic_target_objects.objects.push_back(object);
637-
}
638-
}
618+
const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
619+
*(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_,
620+
planner_data_->parameters.vehicle_width);
639621
const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
640-
dynamic_target_objects, p->th_moving_object_velocity);
622+
dynamic_target_objects, parameters_->th_moving_object_velocity);
641623

642624
// update goal searcher and generate goal candidates
643625
if (goal_candidates_.empty()) {
@@ -753,47 +735,17 @@ bool GoalPlannerModule::isExecutionRequested() const
753735
// check if goal_pose is in current_lanes or neighboring road lanes
754736
const lanelet::ConstLanelets current_lanes =
755737
utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
756-
const auto getNeighboringLane =
757-
[&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
758-
return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
759-
: route_handler->getRightLanelet(lane, false, true);
760-
};
761-
lanelet::ConstLanelets goal_check_lanes = current_lanes;
762-
for (const auto & lane : current_lanes) {
763-
auto neighboring_lane = getNeighboringLane(lane);
764-
while (neighboring_lane) {
765-
goal_check_lanes.push_back(neighboring_lane.value());
766-
neighboring_lane = getNeighboringLane(neighboring_lane.value());
767-
}
768-
}
769-
const bool goal_is_in_current_segment_lanes = std::any_of(
770-
goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
771-
return lanelet::utils::isInLanelet(goal_pose, lane);
772-
});
773-
774-
// check that goal is in current neighbor shoulder lane
775-
const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
776-
for (const auto & lane : current_lanes) {
777-
const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
778-
: route_handler->getRightShoulderLanelet(lane);
779-
if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
780-
return true;
781-
}
782-
}
783-
return false;
784-
});
785-
786-
// if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
787-
// because goal arc coordinates cannot be calculated.
788-
if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) {
738+
const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path(
739+
current_lanes, *(planner_data_->route_handler), left_side_parking_);
740+
if (!is_goal_reachable) {
789741
return false;
790742
}
791743

792744
// if goal modification is not allowed
793745
// 1) goal_pose is in current_lanes, plan path to the original fixed goal
794746
// 2) goal_pose is NOT in current_lanes, do not execute goal_planner
795747
if (!utils::isAllowedGoalModification(route_handler)) {
796-
return goal_is_in_current_segment_lanes;
748+
return is_goal_reachable;
797749
}
798750

799751
// if goal arc coordinates can be calculated, check if goal is in request_length

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

+65
Original file line numberDiff line numberDiff line change
@@ -831,4 +831,69 @@ std::optional<Pose> calcClosestPose(
831831
return closest_pose;
832832
}
833833

834+
autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects(
835+
const autoware_perception_msgs::msg::PredictedObjects & original_objects,
836+
const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters,
837+
const double vehicle_width)
838+
{
839+
const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE;
840+
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
841+
route_handler, left_side_parking, parameters.backward_goal_search_length,
842+
parameters.forward_goal_search_length);
843+
const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
844+
pull_over_lanes, left_side_parking, parameters.detection_bound_offset,
845+
parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width);
846+
847+
PredictedObjects dynamic_target_objects{};
848+
for (const auto & object : original_objects.objects) {
849+
const auto object_polygon = universe_utils::toPolygon2d(object);
850+
if (
851+
objects_extraction_polygon.has_value() &&
852+
boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
853+
dynamic_target_objects.objects.push_back(object);
854+
}
855+
}
856+
return dynamic_target_objects;
857+
}
858+
859+
bool is_goal_reachable_on_path(
860+
const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler,
861+
const bool left_side_parking)
862+
{
863+
const Pose goal_pose = route_handler.getOriginalGoalPose();
864+
const auto getNeighboringLane =
865+
[&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
866+
return left_side_parking ? route_handler.getLeftLanelet(lane, false, true)
867+
: route_handler.getRightLanelet(lane, false, true);
868+
};
869+
lanelet::ConstLanelets goal_check_lanes = current_lanes;
870+
for (const auto & lane : current_lanes) {
871+
auto neighboring_lane = getNeighboringLane(lane);
872+
while (neighboring_lane) {
873+
goal_check_lanes.push_back(neighboring_lane.value());
874+
neighboring_lane = getNeighboringLane(neighboring_lane.value());
875+
}
876+
}
877+
const bool goal_is_in_current_segment_lanes = std::any_of(
878+
goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
879+
return lanelet::utils::isInLanelet(goal_pose, lane);
880+
});
881+
882+
// check that goal is in current neighbor shoulder lane
883+
const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
884+
for (const auto & lane : current_lanes) {
885+
const auto shoulder_lane = left_side_parking ? route_handler.getLeftShoulderLanelet(lane)
886+
: route_handler.getRightShoulderLanelet(lane);
887+
if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
888+
return true;
889+
}
890+
}
891+
return false;
892+
});
893+
894+
// if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
895+
// because goal arc coordinates cannot be calculated.
896+
return goal_is_in_current_segment_lanes || goal_is_in_current_shoulder_lanes;
897+
}
898+
834899
} // namespace autoware::behavior_path_planner::goal_planner_utils

0 commit comments

Comments
 (0)