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(behavior_path_planner): refactor pull_over #1842

Merged
merged 1 commit into from
Sep 12, 2022
Merged

refactor(behavior_path_planner): refactor pull_over #1842

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
6 changes: 2 additions & 4 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,9 +189,7 @@ class PullOverModule : public SceneModuleInterface

PathWithLaneId getReferencePath() const;
PathWithLaneId generateStopPath() const;
lanelet::ConstLanelets getPullOverLanes() const;
std::pair<bool, bool> getSafePath(ShiftParkingPath & safe_path) const;
Pose getRefinedGoal() const;
Pose calcRefinedGoal() const;
Pose getParkingStartPose() const;
ParallelParkingParameters getGeometricPullOverParameters() const;
bool isLongEnoughToParkingStart(
Expand All @@ -202,7 +200,7 @@ class PullOverModule : public SceneModuleInterface
double calcMinimumShiftPathDistance() const;
std::pair<double, double> calcDistanceToPathChange() const;

bool planShiftPath();
bool planShiftPath(const Pose goal_pose);
bool isStopped();
bool hasFinishedCurrentPath();
bool hasFinishedPullOver();
Expand Down
26 changes: 7 additions & 19 deletions planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,37 +43,25 @@ using geometry_msgs::msg::Twist;

// TODO(sugahara) move to util
PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2);
bool isPathInLanelets(
const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
const lanelet::ConstLanelets & target_lanelets);
std::vector<ShiftParkingPath> getShiftParkingPaths(

std::vector<ShiftParkingPath> generateShiftParkingPaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Pose & goal_pose,
const Twist & twist, const BehaviorPathPlannerParameters & common_parameter,
const BehaviorPathPlannerParameters & common_parameter,
const behavior_path_planner::PullOverParameters & parameter);

std::vector<ShiftParkingPath> selectValidPaths(
const std::vector<ShiftParkingPath> & paths, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose,
const bool isInGoalRouteSection, const Pose & goal_pose,
const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
const bool is_in_goal_route_section, const Pose & goal_pose,
const lane_departure_checker::LaneDepartureChecker & lane_departure_checker);
bool selectSafePath(
const std::vector<ShiftParkingPath> & paths,
const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path);
bool isPullOverPathSafe(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width,
const behavior_path_planner::PullOverParameters & ros_parameters, const bool use_buffer = true,
const double acceleration = 0.0);
bool hasEnoughDistance(
const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
const bool isInGoalRouteSection, const Pose & goal_pose,
const lanelet::routing::RoutingGraphContainer & overall_graphs);
bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose);
lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler);
} // namespace pull_over_utils
} // namespace behavior_path_planner

Expand Down
2 changes: 0 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,6 @@ void getProjectedDistancePointFromPolygons(
Pose & point_on_object);
// data conversions

Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);

std::vector<Pose> convertToPoseArray(const PathWithLaneId & path);

std::vector<Point> convertToGeometryPointArray(const PathWithLaneId & path);
Expand Down
12 changes: 2 additions & 10 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -779,20 +779,12 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
const PathWithLaneId & path) const
{
const auto goal = planner_data_->route_handler->getGoalPose();
const auto is_approved = planner_data_->approval.is_approved.data;
auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();

Pose refined_goal{};
{
lanelet::ConstLanelet goal_lanelet;
lanelet::ConstLanelet pull_over_lane;
geometry_msgs::msg::Pose pull_over_goal;
if (
is_approved && planner_data_->route_handler->getPullOverTarget(
planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) {
refined_goal = planner_data_->route_handler->getPullOverGoalPose();
goal_lane_id = pull_over_lane.id();
} else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
refined_goal = util::refineGoal(goal, goal_lanelet);
} else {
refined_goal = goal;
Expand Down
Loading