From e4e5b4850d328d29734340b635c850c8896ceae7 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed, 10 Aug 2022 15:20:29 +0900 Subject: [PATCH] fix(scenario_selector): timing of handling route (#1546) * fix(scenario_selector): timing of handling route Signed-off-by: Shumpei Wakabayashi * feat: add isDataReady Signed-off-by: Shumpei Wakabayashi --- .../scenario_selector_node.hpp | 1 + .../scenario_selector_node.cpp | 38 +++++++++++++++++-- 2 files changed, 35 insertions(+), 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 index 6b405c1b88b07..26f9c0a07e7f5 100644 --- 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 index 029bd2a3cbb90..303c5e0d34e4e 100644 --- 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,44 @@ void ScenarioSelectorNode::onParkingState(const std_msgs::msg::Bool::ConstShared is_parking_completed_ = msg->data; } +bool ScenarioSelectorNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for current pose."); + return false; + } + + if (!lanelet_map_ptr_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for lanelet map."); + return false; + } + + if (!route_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for route."); + return false; + } + + if (!twist_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for twist."); + 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, "Waiting 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; }