diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index 8ab7acd5ec9b3..160ee183d8a0a 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -31,6 +31,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -86,11 +87,15 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; - rclcpp::Subscription::SharedPtr sub_current_odometry_; - rclcpp::Subscription::SharedPtr sub_current_acceleration_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - rclcpp::Subscription::SharedPtr sub_operation_mode_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + this, "/localization/kinematic_state"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_acceleration_{this, "~/input/acceleration"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ + this, "~/input/external_velocity_limit_mps"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; @@ -180,12 +185,8 @@ class VelocitySmootherNode : public rclcpp::Node const std::vector & parameters); // topic callback - void onCurrentOdometry(const Odometry::ConstSharedPtr msg); - void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); - void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); - void calcExternalVelocityLimit(); // publish methods diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 0fd83871153f0..1541b19d70a1c 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -58,19 +58,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); - sub_current_odometry_ = create_subscription( - "/localization/kinematic_state", 1, - std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1)); - sub_external_velocity_limit_ = create_subscription( - "~/input/external_velocity_limit_mps", 1, - std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1)); - sub_current_acceleration_ = create_subscription( - "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { - current_acceleration_ptr_ = msg; - }); - sub_operation_mode_ = create_subscription( - "~/input/operation_mode_state", 1, - [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update set_param_res_ = @@ -319,16 +306,6 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory pub_trajectory_, publishing_trajectory.header.stamp); } -void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) -{ - current_odometry_ptr_ = msg; -} - -void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) -{ - external_velocity_limit_ptr_ = msg; -} - void VelocitySmootherNode::calcExternalVelocityLimit() { if (!external_velocity_limit_ptr_) { @@ -441,6 +418,15 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr base_traj_raw_ptr_ = msg; + // receive data + current_odometry_ptr_ = sub_current_odometry_.takeData(); + current_acceleration_ptr_ = sub_current_acceleration_.takeData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); + const auto operation_mode_ptr = sub_operation_mode_.takeData(); + if (operation_mode_ptr) { + operation_mode_ = *operation_mode_ptr; + } + // guard if (!checkData()) { return;