[app] Retrun to circular if reached circular trajectory

HiroIshida · Apr 29, 2022 · 0dc6762 · 0dc6762
1 parent f4ca770
commit 0dc6762
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 11 deletions.
diff --git a/planning/auto_parking_planner/include/auto_parking_planner.hpp b/planning/auto_parking_planner/include/auto_parking_planner.hpp
@@ -156,6 +156,7 @@ class AutoParkingPlanner : public rclcpp::Node
     const std::shared_ptr<autoware_parking_srvs::srv::ParkingMissionPlan::Request> request,
     std::shared_ptr<autoware_parking_srvs::srv::ParkingMissionPlan::Response> response);
 
+  bool previousRouteFinished() const;
   bool waitUntilPreviousRouteFinished() const;
 
   std::vector<Pose> askFeasibleGoalIndex(

diff --git a/planning/auto_parking_planner/src/auto_parking_planner.cpp b/planning/auto_parking_planner/src/auto_parking_planner.cpp
@@ -173,6 +173,21 @@ bool AutoParkingPlanner::parkingMissionPlanCallback(
   return true;
 }
 
+bool AutoParkingPlanner::previousRouteFinished() const
+{
+  const double dist_error =
+    tier4_autoware_utils::calcDistance2d(getEgoVehiclePose(), previous_route_->goal_pose);
+  RCLCPP_INFO_STREAM(get_logger(), "dist error:" << dist_error);
+  RCLCPP_INFO_STREAM(get_logger(), "aw state:" << sub_msgs_.state_ptr->state);
+  if (sub_msgs_.state_ptr->state == AutowareState::WAITING_FOR_ROUTE) {
+    if (dist_error < 1.5) {
+      RCLCPP_INFO_STREAM(get_logger(), "preivous route finished!");
+      return true;
+    }
+  }
+  return false;
+}
+
 bool AutoParkingPlanner::waitUntilPreviousRouteFinished() const
 {
   RCLCPP_INFO_STREAM(get_logger(), "waiting for preivous route finished...");
@@ -187,20 +202,11 @@ bool AutoParkingPlanner::waitUntilPreviousRouteFinished() const
     }
   }
 
-  while (true) {
+  while (previousRouteFinished()) {
     RCLCPP_INFO_STREAM(get_logger(), "waiting now...");
-    const double dist_error =
-      tier4_autoware_utils::calcDistance2d(getEgoVehiclePose(), previous_route_->goal_pose);
-    RCLCPP_INFO_STREAM(get_logger(), "dist error:" << dist_error);
-    RCLCPP_INFO_STREAM(get_logger(), "aw state:" << sub_msgs_.state_ptr->state);
     rclcpp::sleep_for(std::chrono::milliseconds(300));
-    if (sub_msgs_.state_ptr->state == AutowareState::WAITING_FOR_ROUTE) {
-      if (dist_error < 1.5) {
-        RCLCPP_INFO_STREAM(get_logger(), "preivous route finished!");
-        return true;
-      }
-    }
   }
+  return true;
 }
 
 std::vector<Pose> AutoParkingPlanner::askFeasibleGoalIndex(

diff --git a/planning/auto_parking_planner/src/preparking_route_planning.cpp b/planning/auto_parking_planner/src/preparking_route_planning.cpp
@@ -87,6 +87,11 @@ PlanningResult AutoParkingPlanner::planPreparkingRoute() const
   while (true) {
     rclcpp::sleep_for(std::chrono::milliseconds(100));
 
+    if (!this->previousRouteFinished()) {
+      const std::string message = "Reached the terminal of a circular trajectory";
+      return PlanningResult{true, ParkingMissionPlan::Request::CIRCULAR, HADMapRoute(), message};
+    }
+
     const auto current_pose = getEgoVehiclePose();
     if (!sub_msgs_.velocity_ptr_ || !sub_msgs_.traj_ptr_ || sub_msgs_.traj_ptr_->points.empty()) {
       continue;