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

refactor(goal_planner): refactor isAllowedGoalModification #4856

Merged
merged 2 commits into from
Sep 4, 2023
+21 −41
Merged

refactor(goal_planner): refactor isAllowedGoalModification #4856

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0ad2541
ractor(goal_planner): refactor isAllowedGoalModification
kosuke55 Sep 3, 2023
cf9dc97
fix build
kosuke55 Sep 4, 2023
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
2 changes: 0 additions & 2 deletions ...behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface

private:
std::shared_ptr<GoalPlannerParameters> parameters_;

bool left_side_parking_;
};

} // namespace behavior_path_planner
Expand Down
6 changes: 2 additions & 4 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,8 @@ PredictedObjects filterObjectsByLateralDistance(
const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
const double distance_thresh, const bool filter_inside);

bool isAllowedGoalModification(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
bool checkOriginalGoalIsInShoulder(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);

// debug
MarkerArray createPullOverAreaMarkerArray(
Expand Down
24 changes: 9 additions & 15 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,10 +309,9 @@ bool GoalPlannerModule::isExecutionRequested() const
// if goal arc coordinates can be calculated, check if goal is in request_length
const double self_to_goal_arc_length =
utils::getSignedDistance(current_pose, goal_pose, current_lanes);
const double request_length =
goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)
? calcModuleRequestLength()
: parameters_->minimum_request_length;
const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler)
? calcModuleRequestLength()
: parameters_->minimum_request_length;
if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) {
// if current position is far from goal or behind goal, do not execute goal_planner
return false;
Expand All @@ -321,7 +320,7 @@ bool GoalPlannerModule::isExecutionRequested() const
// if goal modification is not allowed
// 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 (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) {
if (!goal_planner_utils::isAllowedGoalModification(route_handler)) {
return goal_is_in_current_lanes;
}

Expand Down Expand Up @@ -409,8 +408,7 @@ ModuleStatus GoalPlannerModule::updateState()
{
// finish module only when the goal is fixed
if (
!goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_) &&
!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) &&
hasFinishedGoalPlanner()) {
return ModuleStatus::SUCCESS;
}
Expand Down Expand Up @@ -508,7 +506,7 @@ void GoalPlannerModule::generateGoalCandidates()
// calculate goal candidates
const Pose goal_pose = route_handler->getGoalPose();
refined_goal_pose_ = calcRefinedGoal(goal_pose);
if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) {
if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
goal_searcher_->setPlannerData(planner_data_);
goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
} else {
Expand All @@ -528,8 +526,7 @@ BehaviorModuleOutput GoalPlannerModule::plan()

path_reference_ = getPreviousModuleOutput().reference_path;

if (goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
return planWithGoalModification();
} else {
fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput());
Expand Down Expand Up @@ -853,8 +850,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()

path_reference_ = getPreviousModuleOutput().reference_path;

if (goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
return planWaitingApprovalWithGoalModification();
} else {
fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput());
Expand Down Expand Up @@ -1457,9 +1453,7 @@ void GoalPlannerModule::setDebugData()
}
tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
};

if (goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
// Visualize pull over areas
const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow
: createMarkerColor(0.0, 1.0, 0.0, 0.999); // green
Expand Down
11 changes: 3 additions & 8 deletions planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -241,8 +241,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
}

parameters_ = std::make_shared<GoalPlannerParameters>(p);

left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE;
}

void GoalPlannerModuleManager::updateModuleParams(
Expand All @@ -263,8 +261,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const
{
// enable AlwaysExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
return true;
}

Expand All @@ -279,8 +276,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const

// enable SimultaneousExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
return true;
}

Expand All @@ -295,8 +291,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const

// enable SimultaneousExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!goal_planner_utils::isAllowedGoalModification(
planner_data_->route_handler, left_side_parking_)) {
if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
return true;
}

Expand Down
19 changes: 7 additions & 12 deletions planning/behavior_path_planner/src/utils/goal_planner/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,25 +181,20 @@ MarkerArray createGoalCandidatesMarkerArray(
return marker_array;
}

bool isAllowedGoalModification(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking)
bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler)
{
return route_handler->isAllowedGoalModification() ||
checkOriginalGoalIsInShoulder(route_handler, left_side_parking);
return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler);
}

bool checkOriginalGoalIsInShoulder(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking)
bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler)
{
const Pose & goal_pose = route_handler->getGoalPose();
const auto shoulder_lanes = route_handler->getShoulderLanelets();

const lanelet::ConstLanelets pull_over_lanes =
goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking);
lanelet::ConstLanelet target_lane{};
lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
lanelet::ConstLanelet closest_shoulder_lane{};
lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane);

return route_handler->isShoulderLanelet(target_lane) &&
lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1);
return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
}

} // namespace goal_planner_utils
Expand Down