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

fix(start_planner): use waypoints as centerline if available #10238

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+8 −5
Open

fix(start_planner): use waypoints as centerline if available #10238

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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: 1 addition & 1 deletion ...anner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -230,7 +230,7 @@ class StartPlannerModule : public SceneModuleInterface
bool receivedNewRoute() const;

bool isModuleRunning() const;
bool isCurrentPoseOnMiddleOfTheRoad() const;
bool isCurrentPoseOnCenterline() const;

/**
* @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
Expand Down
11 changes: 7 additions & 4 deletions ...ior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -342,12 +342,12 @@ bool StartPlannerModule::isExecutionRequested() const
}

// Return false and do not request execution if any of the following conditions are true:
// - The start pose is on the middle of the road.
// - The start pose is on the centerline or on "waypoints" (custom centerline)
// - The vehicle has already arrived at the start position planner.
// - The vehicle has reached the goal position.
// - The vehicle is still moving.
if (
isCurrentPoseOnMiddleOfTheRoad() || isCloseToOriginalStartPose() || hasArrivedAtGoal() ||
isCurrentPoseOnCenterline() || isCloseToOriginalStartPose() || hasArrivedAtGoal() ||
isMoving()) {
return false;
}
Expand All @@ -367,12 +367,15 @@ bool StartPlannerModule::isModuleRunning() const
return getCurrentStatus() == ModuleStatus::RUNNING;
}

bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const
bool StartPlannerModule::isCurrentPoseOnCenterline() const
{
const auto & lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
const double lateral_distance_to_center_lane =
lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
lanelet::utils::getArcCoordinatesConsideringWaypoints(
current_lanes, current_pose, lanelet_map_ptr)
.distance;

return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road;
}
Expand Down
Loading