feat(autoware_control_validator): add polling subcribers (#7426)

* add polling subs Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * delete extra line Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
autowarefoundation · Jul 22, 2024 · fba4ca4 · fba4ca4
1 parent e97e7e2
commit fba4ca4
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 22 deletions.
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
@@ -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 <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -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<Odometry>::SharedPtr sub_kinematics_;
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_traj_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
+    this, "~/input/kinematics"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_traj_{
+    this, "~/input/reference_trajectory"};
   rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
 

diff --git 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<Odometry>(
-    "~/input/kinematics", 1,
-    [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; });
-  sub_reference_traj_ = create_subscription<Trajectory>(
-    "~/input/reference_trajectory", 1,
-    std::bind(&ControlValidator::onReferenceTrajectory, this, _1));
   sub_predicted_traj_ = create_subscription<Trajectory>(
     "~/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;