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]>
kyoichi-sugahara · Nov 7, 2023 · bd3b6fb · bd3b6fb
1 parent f662ac7
commit bd3b6fb
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 44 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 isStartPoseOnMiddleOfTheRoad() const;
+  bool hasArrivedAtStart() 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/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -145,41 +145,64 @@ 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();
-  if (
-    tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) >
-    parameters_->th_arrived_distance) {
+  // 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 (isStartPoseOnMiddleOfTheRoad() || hasArrivedAtStart() || 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::isStartPoseOnMiddleOfTheRoad() 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::hasArrivedAtStart() 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 +257,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 +399,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(