feat(behavior_path_planner): change execution requested condition (au…

…towarefoundation#5504) * refactor(start-planner-module): Improve code readability - Create separate methods to check if the module is running, if the start pose is on the middle of the road, if the vehicle has arrived at the start position, if the vehicle has arrived at the goal position, and if the vehicle is still moving. - Add a warning message if the goal is behind the ego vehicle within the same route segment. Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
zulfaqar-azmi-t4 · Nov 7, 2023 · df0b857 · df0b857
1 parent 05396ea
commit df0b857
Show file tree

Hide file tree

Showing 4 changed files with 60 additions and 43 deletions.
diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...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
@@ -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
@@ -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
@@ -974,6 +974,9 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerDat
     shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);
 
+  // TODO(someone): insert 0 velocity on the path
+  // assumption: this function is called when any of the scene module is running and ego is outside
+  // of the route
   output.path = std::make_shared<PathWithLaneId>(reference_path);
   output.reference_path = std::make_shared<PathWithLaneId>(reference_path);
   output.drivable_area_info.drivable_lanes = drivable_lanes;