replace rclcpp::Subscription to tier4_autoware_utils::InterProcessPol…

…lingSubscriber

Signed-off-by: Autumn60 <[email protected]>

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

@@ -102,6 +102,13 @@ ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent
 void ManualController::update()
 {
   if (!raw_node_) return;
+
+  const auto velocity = sub_velocity_->takeData();
+  const double current_velocity = velocity ? velocity->longitudinal_velocity : 0.0;
+
+  const auto accel = sub_accel_->takeData();
+  const double current_acceleration = accel ? accel->accel.accel.linear.x : 0.0;
+
   Control control_cmd;
   {
     control_cmd.stamp = raw_node_->get_clock()->now();
@@ -114,18 +121,18 @@ void ManualController::update()
      *  a_des = k_const *(v - v_des) + a (k < 0 )
      */
     const double k = -0.5;
-    const double v = current_velocity_;
+    const double v = current_velocity;
     const double v_des = cruise_velocity_;
-    const double a = current_acceleration_;
+    const double a = current_acceleration;
     const double a_des = k * (v - v_des) + a;
     control_cmd.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
   }
   GearCommand gear_cmd;
   {
     const double eps = 0.001;
-    if (control_cmd.longitudinal.velocity > eps && current_velocity_ > -eps) {
+    if (control_cmd.longitudinal.velocity > eps && current_velocity > -eps) {
       gear_cmd.command = GearCommand::DRIVE;
-    } else if (control_cmd.longitudinal.velocity < -eps && current_velocity_ < eps) {
+    } else if (control_cmd.longitudinal.velocity < -eps && current_velocity < eps) {
       gear_cmd.command = GearCommand::REVERSE;
       control_cmd.longitudinal.acceleration *= -1.0;
     } else {
@@ -157,8 +164,12 @@ void ManualController::onInitialize()
   sub_gate_mode_ = raw_node_->create_subscription<GateMode>(
     "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1));
 
-  sub_velocity_ = raw_node_->create_subscription<VelocityReport>(
-    "/vehicle/status/velocity_status", 1, std::bind(&ManualController::onVelocity, this, _1));
+  sub_velocity_ =
+    tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport>::create_subscription(
+      raw_node_.get(), "/vehicle/status/velocity_status", 1);
+
+  sub_accel_ = tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>::
+    create_subscription(raw_node_.get(), "/localization/acceleration", 1);
 
   sub_engage_ = raw_node_->create_subscription<Engage>(
     "/api/autoware/get/engage", 10, std::bind(&ManualController::onEngageStatus, this, _1));
@@ -207,16 +218,6 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg)
   }
 }
 
-void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg)
-{
-  current_velocity_ = msg->longitudinal_velocity;
-}
-
-void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
-{
-  current_acceleration_ = msg->accel.accel.linear.x;
-}
-
 void ManualController::onGear(const GearReport::ConstSharedPtr msg)
 {
   switch (msg->report) {

common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp

@@ -23,6 +23,7 @@
 #include <QSpinBox>
 #include <rclcpp/rclcpp.hpp>
 #include <rviz_common/panel.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 
 #include "autoware_vehicle_msgs/msg/velocity_report.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
@@ -68,13 +69,13 @@ public Q_SLOTS:  // NOLINT for Qt
   void onTimer();
   void onPublishControlCommand();
   void onGateMode(const GateMode::ConstSharedPtr msg);
-  void onVelocity(const VelocityReport::ConstSharedPtr msg);
-  void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
   void onEngageStatus(const Engage::ConstSharedPtr msg);
   void onGear(const GearReport::ConstSharedPtr msg);
   rclcpp::Node::SharedPtr raw_node_;
   rclcpp::Subscription<GateMode>::SharedPtr sub_gate_mode_;
-  rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport>::SharedPtr sub_velocity_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>::SharedPtr
+    sub_accel_;
   rclcpp::Subscription<Engage>::SharedPtr sub_engage_;
   rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
   rclcpp::Publisher<Control>::SharedPtr pub_control_command_;
@@ -84,8 +85,6 @@ public Q_SLOTS:  // NOLINT for Qt
 
   double cruise_velocity_{0.0};
   double steering_angle_{0.0};
-  double current_velocity_{0.0};
-  double current_acceleration_{0.0};
 
   QLabel * gate_mode_label_ptr_;
   QLabel * gear_label_ptr_;