diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 206ca5ea080c3..8ea963fec0b8d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -132,6 +132,12 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); + bool isModuleRunning() const; + bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isCloseToOriginalStartPose() const; + bool hasArrivedAtGoal() const; + bool isMoving() const; + PriorityOrder determinePriorityOrder( const std::string & search_priority, const size_t candidates_size); bool findPullOutPath( diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 3c470cef2c935..6d34ce4ecde38 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -300,6 +300,8 @@ bool GoalPlannerModule::isExecutionRequested() const return true; } + // TODO(someone): if goal is behind of ego, do not execute goal_planner + const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose goal_pose = route_handler->getOriginalGoalPose(); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 622cc9e3b87ae..e3670710b64f0 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -145,41 +145,66 @@ void StartPlannerModule::updateData() bool StartPlannerModule::isExecutionRequested() const { - 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; - - if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { - return false; + if (isModuleRunning()) { + return true; } - const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); + // 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 vehicle has already arrived at the start position planner. + // - The vehicle has reached the goal position. + // - The vehicle is still moving. if ( - tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > - parameters_->th_arrived_distance) { + isCurrentPoseOnMiddleOfTheRoad() || isCloseToOriginalStartPose() || hasArrivedAtGoal() || + isMoving()) { return false; } - // Check if ego arrives at goal - const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - if ( - tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < - parameters_->th_arrived_distance) { + // Check if the goal is behind the ego vehicle within the same route segment. + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); return false; } - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } + return true; +} - const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < - parameters_->th_stopped_velocity; - if (!is_stopped) { - return false; - } +bool StartPlannerModule::isModuleRunning() const +{ + return current_state_ == ModuleStatus::RUNNING; +} - return true; +bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const +{ + 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; + + return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; +} + +bool StartPlannerModule::isCloseToOriginalStartPose() const +{ + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); + return tier4_autoware_utils::calcDistance2d( + start_pose.position, planner_data_->self_odometry->pose.pose.position) > + parameters_->th_arrived_distance; +} + +bool StartPlannerModule::hasArrivedAtGoal() const +{ + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); + return tier4_autoware_utils::calcDistance2d( + goal_pose.position, planner_data_->self_odometry->pose.pose.position) < + parameters_->th_arrived_distance; +} + +bool StartPlannerModule::isMoving() const +{ + return utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) >= + parameters_->th_stopped_velocity; } bool StartPlannerModule::isExecutionReady() const @@ -234,15 +259,6 @@ void StartPlannerModule::updateCurrentState() BehaviorModuleOutput StartPlannerModule::plan() { - if (IsGoalBehindOfEgoInSameRouteSegment()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - const auto output = generateStopOutput(); - setDebugData(); // use status updated in generateStopOutput() - updateRTCStatus(0, 0); - return output; - } - if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); @@ -385,16 +401,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); - if (IsGoalBehindOfEgoInSameRouteSegment()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - clearWaitingApproval(); - const auto output = generateStopOutput(); - setDebugData(); // use status updated in generateStopOutput() - updateRTCStatus(0, 0); - return output; - } - BehaviorModuleOutput output; if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 93f74e670ecdd..62a48da85ba2c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -974,6 +974,9 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); output.reference_path = std::make_shared(reference_path); output.drivable_area_info.drivable_lanes = drivable_lanes;