diff --git a/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp index d26abfc2ecd2f..2e31de295f1f2 100644 --- a/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp @@ -18,6 +18,7 @@ #include "autoware_control_validator/debug_marker.hpp" #include "autoware_control_validator/msg/control_validator_status.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -48,7 +49,6 @@ class ControlValidator : public rclcpp::Node public: explicit ControlValidator(const rclcpp::NodeOptions & options); - void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); @@ -68,9 +68,11 @@ class ControlValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_reference_traj_; rclcpp::Subscription::SharedPtr sub_predicted_traj_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_traj_{ + this, "~/input/reference_trajectory"}; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index a0873f93c1e16..7e4a86d42598e 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -28,13 +28,6 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) : Node("control_validator", options) { using std::placeholders::_1; - - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); - sub_reference_traj_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&ControlValidator::onReferenceTrajectory, this, _1)); sub_predicted_traj_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); @@ -116,21 +109,11 @@ bool ControlValidator::isDataReady() return true; } -void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - if (msg->points.size() < 2) { - RCLCPP_ERROR(get_logger(), "planning trajectory size is invalid (%lu)", msg->points.size()); - return; - } - - current_reference_trajectory_ = msg; - - return; -} - void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) { current_predicted_trajectory_ = msg; + current_reference_trajectory_ = sub_reference_traj_.takeData(); + current_kinematics_ = sub_kinematics_.takeData(); if (!isDataReady()) return;