feat(autoware_control_validator): add polling subcribers (autowarefou…

…ndation#7426) * add polling subs Signed-off-by: Daniel Sanchez <[email protected]> * delete extra line Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
NorahXiong · Jun 11, 2024 · 32274ff · 32274ff
1 parent e3a30cd
commit 32274ff
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;