From fd7654b08b4161b1183b15152c92c274cabb5742 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Aug 2022 14:48:09 +0900 Subject: [PATCH] fix(behavior_path_planner): pull_over far goal bug (#1571) (#93) Signed-off-by: kosuke55 Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over_module.cpp | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index a38b1b3aa4300..7c46baa7771b1 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -157,21 +157,23 @@ bool PullOverModule::isExecutionRequested() const if (current_state_ == BT::NodeStatus::RUNNING) { return true; } - - const auto current_lanes = util::getCurrentLanes(planner_data_); - const auto goal_pose = planner_data_->route_handler->getGoalPose(); + const auto & current_lanes = util::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_pose->pose; + const auto & goal_pose = planner_data_->route_handler->getGoalPose(); // check if goal_pose is far - const double goal_arc_length = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length; - if (std::abs(goal_arc_length) < std::numeric_limits::epsilon()) { - // can not caluculate arc langth correctly, maybe lane loop. + const bool is_in_goal_route_section = + planner_data_->route_handler->isInGoalRouteSection(current_lanes.back()); + // current_lanes does not have the goal + if (!is_in_goal_route_section) { + return false; + } + const double self_to_goal_arc_length = + util::getSignedDistance(current_pose, goal_pose, current_lanes); + // goal is away behind + if (self_to_goal_arc_length > parameters_.request_length || self_to_goal_arc_length < 0.0) { return false; } - - const double self_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose).length; - const double self_to_goal_arc_length = goal_arc_length - self_arc_length; - if (self_to_goal_arc_length > parameters_.request_length) return false; // check if goal_pose is in shoulder lane bool goal_is_in_shoulder_lane = false;