fix(behavior_path_planner): remove waitForData() in constructor (tier…

…4#1684)

* fix(behavior_path_planner): remove waitForData() in constructor

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add rclcpp_info for missing messages

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -93,7 +93,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   std::mutex mutex_bt_;  // mutex for bt_manager_
 
   // setup
-  void waitForData();
+  bool isDataReady();
 
   // parameters
   BehaviorPathPlannerParameters getCommonParam();

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -146,9 +146,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       planner_data_->parameters.base_link2front, intersection_search_distance);
   }
 
-  waitForData();
-
-  // Start timer. This must be done after all data (e.g. vehicle pose, velocity) are ready.
+  // Start timer
   {
     const auto planning_hz = declare_parameter("planning_hz", 10.0);
     const auto period_ns = rclcpp::Rate(planning_hz).period();
@@ -494,46 +492,47 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam()
   return p;
 }
 
-void BehaviorPathPlannerNode::waitForData()
+// wait until mandatory data is ready
+bool BehaviorPathPlannerNode::isDataReady()
 {
-  // wait until mandatory data is ready
-  while (!current_scenario_ && rclcpp::ok()) {
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for scenario topic");
-    rclcpp::spin_some(this->get_node_base_interface());
-    rclcpp::Rate(100).sleep();
-  }
+  const auto missing = [this](const auto & name) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name);
+    mutex_pd_.unlock();
+    return false;
+  };
 
   mutex_pd_.lock();  // for planner_data_
-  while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
-    mutex_pd_.unlock();
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(
-      get_logger(), *get_clock(), 5000, "waiting for route to be ready");
-    rclcpp::spin_some(this->get_node_base_interface());
-    rclcpp::Rate(100).sleep();
-    mutex_pd_.lock();
+  if (!current_scenario_) {
+    return missing("scenario_topic");
   }
 
-  while (rclcpp::ok()) {
-    if (planner_data_->dynamic_object && planner_data_->self_odometry) {
-      break;
-    }
+  if (!planner_data_->route_handler->isHandlerReady()) {
+    return missing("route");
+  }
 
-    mutex_pd_.unlock();
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(
-      get_logger(), *get_clock(), 5000,
-      "waiting for vehicle pose, vehicle_velocity, and obstacles");
-    rclcpp::spin_some(this->get_node_base_interface());
-    rclcpp::Rate(100).sleep();
-    mutex_pd_.lock();
+  if (!planner_data_->dynamic_object) {
+    return missing("dynamic_object");
+  }
+
+  if (!planner_data_->self_odometry) {
+    return missing("self_odometry");
   }
 
-  self_pose_listener_.waitForFirstPose();
   planner_data_->self_pose = self_pose_listener_.getCurrentPose();
+  if (!planner_data_->self_pose) {
+    return missing("self_pose");
+  }
+
   mutex_pd_.unlock();
+  return true;
 }
 
 void BehaviorPathPlannerNode::run()
 {
+  if (!isDataReady()) {
+    return;
+  }
+
   RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----");
   mutex_bt_.lock();  // for bt_manager_
   mutex_pd_.lock();  // for planner_data_