From aac23c9d7bbe49b789fb340c5ce5d1307718fc17 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 2 Aug 2022 15:36:40 +0900 Subject: [PATCH] feat(behavior_velocity_planner): output missing inputs (#1477) * feat(behavior_velocity_planner): output missing inputs Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --- .../include/behavior_velocity_planner/node.hpp | 2 +- planning/behavior_velocity_planner/src/node.cpp | 16 ++++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp index 9c0a42d0c56cf..68e0e460dc4e3 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -113,7 +113,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData planner_data) const; + bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ae534df785779..587716670e62d 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -203,35 +203,46 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } // NOTE: argument planner_data must not be referenced for multithreading -bool BehaviorVelocityPlannerNode::isDataReady(const PlannerData planner_data) const +bool BehaviorVelocityPlannerNode::isDataReady( + const PlannerData planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; // from tf if (d.current_pose.header.frame_id == "") { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Frame id of current pose is missing"); return false; } // from callbacks if (!d.current_velocity) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity"); return false; } if (!d.current_accel) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration"); return false; } if (!d.predicted_objects) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for predicted_objects"); return false; } if (!d.no_ground_pointcloud) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud"); return false; } if (!d.route_handler_) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, 3000, "Waiting for the initialization of route_handler"); return false; } if (!d.route_handler_->isMapMsgReady()) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map"); return false; } if (!d.velocity_smoother_) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother"); return false; } return true; @@ -381,6 +392,7 @@ void BehaviorVelocityPlannerNode::onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { mutex_.lock(); // for planner_data_ + // Check ready try { planner_data_.current_pose = @@ -391,7 +403,7 @@ void BehaviorVelocityPlannerNode::onTrigger( return; } - if (!isDataReady(planner_data_)) { + if (!isDataReady(planner_data_, *get_clock())) { mutex_.unlock(); return; }