diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp index a5ae31133be8e..10d2df8285f12 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp @@ -34,6 +34,7 @@ #include "pure_pursuit/pure_pursuit_viz.hpp" #include +#include #include #include @@ -48,7 +49,6 @@ #include #include -#include namespace pure_pursuit { @@ -79,8 +79,10 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr sub_current_odometry_; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_trajectory_{this, "input/reference_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_odometry_{this, "input/current_odometry"}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index a4b491930df1e..01db317699024 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -71,13 +71,6 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) param_.reverse_min_lookahead_distance = this->declare_parameter("reverse_min_lookahead_distance"); - // Subscribers - using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( - "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); - sub_current_odometry_ = this->create_subscription( - "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); - // Publishers pub_ctrl_cmd_ = this->create_publisher("output/control_raw", 1); @@ -118,21 +111,12 @@ bool PurePursuitNode::isDataReady() return true; } -void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odometry_ = msg; -} - -void PurePursuitNode::onTrajectory( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) -{ - trajectory_ = msg; -} - void PurePursuitNode::onTimer() { current_pose_ = self_pose_listener_.getCurrentPose(); + current_odometry_ = sub_current_odometry_.takeData(); + trajectory_ = sub_trajectory_.takeData(); if (!isDataReady()) { return; }