diff --git a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index fa0b116eafe86..b2a9c8f2e94a4 100644 --- a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -19,6 +19,7 @@ #include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -102,7 +103,8 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - rclcpp::Subscription::SharedPtr sub_kinematics_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::SharedPtr pub_status_; diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 420ee564cda72..9f54547eb62e1 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -32,9 +32,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) { using std::placeholders::_1; - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); sub_traj_ = create_subscription( "~/input/trajectory", 1, std::bind(&PlanningValidator::onTrajectory, this, _1)); @@ -195,6 +192,9 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) current_trajectory_ = msg; + // receive data + current_kinematics_ = sub_kinematics_.takeData(); + if (!isDataReady()) return; if (publish_diag_ && !diag_updater_) {