minor refactor

autowarefoundation · Jan 16, 2022 · acf6e23 · acf6e23
1 parent aaae68c
commit acf6e23
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 15 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -99,9 +99,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   TurnSignalDecider turn_signal_decider_;
 
-  // setup
-  void waitForData();
-
   // parameters
   BehaviorPathPlannerParameters getCommonParam();
   BehaviorTreeManagerParam getBehaviorTreeManagerParam();
@@ -116,6 +113,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void setRoute(const HADMapRoute route);
   void setPlanningData(const PlanningData planning_data);
 
+  void waitForRouteHandler();
+
   /**
    * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point.
    */

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -401,15 +401,6 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam()
   return p;
 }
 
-void BehaviorPathPlannerNode::waitForData()
-{
-  while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
-    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready");
-    rclcpp::spin_some(this->get_node_base_interface());
-    rclcpp::Rate(100).sleep();
-  }
-}
-
 void BehaviorPathPlannerNode::onPlanService(
   const planning_manager::srv::BehaviorPathPlannerPlan::Request::SharedPtr request,
   const planning_manager::srv::BehaviorPathPlannerPlan::Response::SharedPtr response)
@@ -420,9 +411,6 @@ void BehaviorPathPlannerNode::onPlanService(
   setRoute(request->route);
   setPlanningData(request->planning_data);
 
-  // wait for planning data to be ready
-  waitForData();
-
   // update planner data
   updateCurrentPose();
 
@@ -641,6 +629,18 @@ void BehaviorPathPlannerNode::setPlanningData([[maybe_unused]] const PlanningDat
   planner_data_->approval.is_force_approved.module_name =
     getModuleName(planning_data.force_approval.module);
   planner_data_->approval.is_force_approved.stamp = planning_data.header.stamp;
+
+  // wait for route handler to be ready
+  waitForRouteHandler();
+}
+
+void BehaviorPathPlannerNode::waitForRouteHandler()
+{
+  while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
+    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready");
+    rclcpp::spin_some(this->get_node_base_interface());
+    rclcpp::Rate(100).sleep();
+  }
 }
 
 void BehaviorPathPlannerNode::clipPathLength(PathWithLaneId & path) const