From 3f8f5c826306b41d0fe03c212771750810700203 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 6 Jun 2024 18:07:25 +0900 Subject: [PATCH 1/5] 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 680c4a3cdfff1..4a3df99a79e66 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/5] 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 4a3df99a79e66..8ddde943e0b43 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 ab1c35e246719..c7671ad21a79e 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/5] 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 8ddde943e0b43..00c510059004e 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/5] 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 00c510059004e..2be2b47cbb156 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 cda066e00f88debb5584a162f17bad23dfce2793 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Tue, 11 Jun 2024 15:13:45 +0900 Subject: [PATCH 5/5] unified subscribing Signed-off-by: Autumn60 --- .../goal_distance_calculator_node.hpp | 1 + .../src/goal_distance_calculator_node.cpp | 5 +++-- 2 files changed, 4 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 2be2b47cbb156..51ed44bd97062 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 @@ -51,6 +51,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_; // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; 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 c7671ad21a79e..dbd737371fcaa 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -59,7 +59,7 @@ bool GoalDistanceCalculatorNode::isDataReady() return false; } - if (!sub_route_.takeData()) { + if (route_) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); return false; } @@ -81,6 +81,7 @@ bool GoalDistanceCalculatorNode::isDataTimeout() void GoalDistanceCalculatorNode::onTimer() { current_pose_ = self_pose_listener_.getCurrentPose(); + route_ = sub_route_.takeData(); if (!isDataReady()) { return; @@ -91,7 +92,7 @@ void GoalDistanceCalculatorNode::onTimer() } input_.current_pose = current_pose_; - input_.route = sub_route_.takeData(); + input_.route = route_; output_ = goal_distance_calculator_->update(input_);