fix(scenario_selector): timing of handling route

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>
autowarefoundation · Aug 9, 2022 · 1140e4a · 1140e4a
1 parent 0fe0931
commit 1140e4a
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 4 deletions.
diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp
@@ -52,6 +52,7 @@ class ScenarioSelectorNode : public rclcpp::Node
   void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
   void onParkingState(const std_msgs::msg::Bool::ConstSharedPtr msg);
 
+  bool isDataReady();
   void onTimer();
   void onLaneDrivingTrajectory(
     const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);

diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp
@@ -219,8 +219,6 @@ void ScenarioSelectorNode::onMap(
 void ScenarioSelectorNode::onRoute(
   const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg)
 {
-  route_handler_->setRoute(*msg);
-  if (!route_handler_->isHandlerReady()) return;
   route_ = msg;
   current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY;
 }
@@ -252,12 +250,31 @@ void ScenarioSelectorNode::onParkingState(const std_msgs::msg::Bool::ConstShared
   is_parking_completed_ = msg->data;
 }
 
+bool ScenarioSelectorNode::isDataReady()
+{
+  // Check all inputs are ready
+  if (!current_pose_ || !lanelet_map_ptr_ || !route_ || !twist_) {
+    RCLCPP_INFO_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "Wait until ready for all input.");
+    return false;
+  }
+
+  // Check route handler is ready
+  route_handler_->setRoute(*route_);
+  if (!route_handler_->isHandlerReady()) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "Wait until ready for route handler.");
+    return false;
+  }
+
+  return true;
+}
+
 void ScenarioSelectorNode::onTimer()
 {
   current_pose_ = getCurrentPose(tf_buffer_, this->get_logger());
 
-  // Check all inputs are ready
-  if (!current_pose_ || !lanelet_map_ptr_ || !route_ || !twist_) {
+  if (!isDataReady()) {
     return;
   }