From 3f8f5c826306b41d0fe03c212771750810700203 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 6 Jun 2024 18:07:25 +0900 Subject: [PATCH 1/6] delete unnecessary member Signed-off-by: Autumn60 --- .../goal_distance_calculator/goal_distance_calculator_node.hpp | 1 - 1 file changed, 1 deletion(-) 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 680c4a3cdfff..4a3df99a79e6 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 @@ -44,7 +44,6 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_initial_pose_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; rclcpp::Subscription::SharedPtr sub_route_; From ebdd17e81abf62e91c2ce7eefb671cc75fdd4903 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 6 Jun 2024 18:17:03 +0900 Subject: [PATCH 2/6] replace rclcpp::Subscription to autoware_utils::InterProcessPollingSubscriber Signed-off-by: Autumn60 --- .../goal_distance_calculator_node.hpp | 5 +++-- .../src/goal_distance_calculator_node.cpp | 15 ++------------- 2 files changed, 5 insertions(+), 15 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 4a3df99a79e6..8ddde943e0b4 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 @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -45,11 +46,11 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + this, "/planning/mission_planning/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); 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 ab1c35e24671..c7671ad21a79 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(); @@ -70,7 +59,7 @@ bool GoalDistanceCalculatorNode::isDataReady() return false; } - if (!route_) { + if (!sub_route_.takeData()) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); return false; } @@ -102,7 +91,7 @@ void GoalDistanceCalculatorNode::onTimer() } input_.current_pose = current_pose_; - input_.route = route_; + input_.route = sub_route_.takeData(); output_ = goal_distance_calculator_->update(input_); From 2b723be151e1ac829a61a845d90773ddf052f8a9 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 6 Jun 2024 18:34:54 +0900 Subject: [PATCH 3/6] format by clang-format Signed-off-by: Autumn60 --- .../goal_distance_calculator_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 8ddde943e0b4..00c510059004 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 @@ -46,8 +46,8 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ - this, "/planning/mission_planning/route"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_route_{this, "/planning/mission_planning/route"}; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; From 012c23e801a720515013ada03c98006712996c9f Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Tue, 11 Jun 2024 13:26:07 +0900 Subject: [PATCH 4/6] delete unnecessary callback func Signed-off-by: Autumn60 --- .../goal_distance_calculator/goal_distance_calculator_node.hpp | 3 --- 1 file changed, 3 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 00c510059004..2be2b47cbb15 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 @@ -52,9 +52,6 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - // Callback - void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); - // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; From 7bc2b3e383ef6b30f18d9a64f70f3d8fa9f02b3d Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Tue, 11 Jun 2024 15:11:35 +0900 Subject: [PATCH 5/6] refactor goal_distance_calculator_node Signed-off-by: Autumn60 --- .../goal_distance_calculator_node.hpp | 9 +-- .../src/goal_distance_calculator_node.cpp | 57 ++++++++++--------- 2 files changed, 32 insertions(+), 34 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 2be2b47cbb15..baf3dac916b0 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 @@ -49,17 +49,14 @@ class GoalDistanceCalculatorNode : public rclcpp::Node tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{this, "/planning/mission_planning/route"}; - // Data Buffer - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; // 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 @@ -67,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 c7671ad21a79..b2a92e16aa70 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -51,53 +51,56 @@ 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 (!sub_route_.takeData()) { - 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 = sub_route_.takeData(); + // 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 tier4_autoware_utils::rad2deg; - const auto & deviation = output_.goal_deviation; + const auto & deviation = output.goal_deviation; debug_publisher_.publish( "deviation/lateral", deviation.lateral); From c4ff6e90a739e759a846d2a767ae25014cfd3639 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 12 Jun 2024 09:37:01 +0900 Subject: [PATCH 6/6] clang format Signed-off-by: Autumn60 --- .../src/goal_distance_calculator_node.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 b2a92e16aa70..ed8820f889bc 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -73,15 +73,13 @@ void GoalDistanceCalculatorNode::onTimer() { Input input = Input(); - if(!tryGetCurrentPose(input.current_pose)) - { + if (!tryGetCurrentPose(input.current_pose)) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); return; } - if(!tryGetRoute(input.route)) - { + if (!tryGetRoute(input.route)) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); return; }