From 9213dc59375e1a39e58611c96eb608da9e1ebc8c Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 20 Jun 2024 14:07:25 +0900 Subject: [PATCH] refactor(goal_distance_calculator): narrow variable scopes and change to read topic by polling (#7434) * delete unnecessary member Signed-off-by: Autumn60 * replace rclcpp::Subscription to autoware_utils::InterProcessPollingSubscriber Signed-off-by: Autumn60 * format by clang-format Signed-off-by: Autumn60 * delete unnecessary callback func Signed-off-by: Autumn60 * refactor goal_distance_calculator_node Signed-off-by: Autumn60 * clang format Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 Co-authored-by: Autumn60 Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Signed-off-by: Simon Eisenmann --- .../goal_distance_calculator_node.hpp | 18 ++--- .../src/goal_distance_calculator_node.cpp | 66 ++++++++----------- 2 files changed, 33 insertions(+), 51 deletions(-) diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 037841a505017..602688ffe51d7 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -18,6 +18,7 @@ #include "goal_distance_calculator/goal_distance_calculator.hpp" #include +#include #include #include @@ -44,16 +45,9 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_initial_pose_; autoware::universe_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; - - // Data Buffer - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; - - // Callback - void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); + autoware::universe_utils::InterProcessPollingSubscriber + sub_route_{this, "/planning/mission_planning/route"}; // Publisher autoware::universe_utils::DebugPublisher debug_publisher_; @@ -61,8 +55,8 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Timer rclcpp::TimerBase::SharedPtr timer_; - bool isDataReady(); - bool isDataTimeout(); + bool tryGetCurrentPose(geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose); + bool tryGetRoute(autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route); void onTimer(); // Parameter @@ -70,8 +64,6 @@ class GoalDistanceCalculatorNode : public rclcpp::Node Param param_; // Core - Input input_; - Output output_; std::unique_ptr goal_distance_calculator_; }; } // namespace goal_distance_calculator diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index 8e4e4c90bc470..24d54ee2fcf87 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -33,12 +33,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions self_pose_listener_(this), debug_publisher_(this, "goal_distance_calculator") { - using std::placeholders::_1; - - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - durable_qos.transient_local(); - // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.oneshot = declare_parameter("oneshot"); @@ -47,11 +41,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_ = std::make_unique(); goal_distance_calculator_->setParam(param_); - // Subscriber - sub_route_ = create_subscription( - "/planning/mission_planning/route", queue_size, - [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); - // Wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -62,53 +51,54 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_ = std::make_unique(); } -bool GoalDistanceCalculatorNode::isDataReady() +bool GoalDistanceCalculatorNode::tryGetCurrentPose( + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose) { - if (!current_pose_) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); - return false; - } - - if (!route_) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); - return false; - } - + auto current_pose_tmp = self_pose_listener_.getCurrentPose(); + if (!current_pose_tmp) return false; + current_pose = current_pose_tmp; return true; } -bool GoalDistanceCalculatorNode::isDataTimeout() +bool GoalDistanceCalculatorNode::tryGetRoute( + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route) { - constexpr double th_pose_timeout = 1.0; - const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now(); - if (pose_time_diff.seconds() > th_pose_timeout) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose is timeout..."); - return true; - } - return false; + auto route_tmp = sub_route_.takeData(); + if (!route_tmp) return false; + route = route_tmp; + return true; } void GoalDistanceCalculatorNode::onTimer() { - current_pose_ = self_pose_listener_.getCurrentPose(); + Input input = Input(); - if (!isDataReady()) { + if (!tryGetCurrentPose(input.current_pose)) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); return; } - if (isDataTimeout()) { + if (!tryGetRoute(input.route)) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); return; } - input_.current_pose = current_pose_; - input_.route = route_; + // Check pose timeout + { + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(input.current_pose->header.stamp) - now(); + if (pose_time_diff.seconds() > th_pose_timeout) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose is timeout..."); + return; + } + } - output_ = goal_distance_calculator_->update(input_); + Output output = goal_distance_calculator_->update(input); { using autoware::universe_utils::rad2deg; - const auto & deviation = output_.goal_deviation; + const auto & deviation = output.goal_deviation; debug_publisher_.publish( "deviation/lateral", deviation.lateral);