feat(raw_vehicle_cmd_converter): use polling subscriber (#7319)

zusizusi · shmpwk · web-flow · commit aa243da0378e · 2024-06-11T18:23:30.000+09:00
* replace subscription

Signed-off-by: Sho Iwasawa &lt;sho.iwasawa.2@tier4.jp&gt;

* fix document

Signed-off-by: Sho Iwasawa &lt;sho.iwasawa.2@tier4.jp&gt;

* sum up functions

Signed-off-by: Sho Iwasawa &lt;sho.iwasawa.2@tier4.jp&gt;

* add maintainer

Signed-off-by: Sho Iwasawa &lt;sho.iwasawa.2@tier4.jp&gt;

---------

Signed-off-by: Sho Iwasawa &lt;sho.iwasawa.2@tier4.jp&gt;
Co-authored-by: Shumpei Wakabayashi &lt;42209144+shmpwk@users.noreply.github.com&gt;
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md
@@ -26,11 +26,11 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration
 
 ## Input topics
 
-| Name                  | Type                                  | Description                                                                                                        |
-| --------------------- | ------------------------------------- | ------------------------------------------------------------------------------------------------------------------ |
-| `~/input/control_cmd` | autoware_control_msgs::msg::Control   | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
-| `~/input/steering"`   | autoware_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control                                                     |
-| `~/input/twist`       | navigation_msgs::Odometry             | twist topic in odometry is used.                                                                                   |
+| Name                  | Type                                       | Description                                                                                                        |
+| --------------------- | ------------------------------------------ | ------------------------------------------------------------------------------------------------------------------ |
+| `~/input/control_cmd` | autoware_control_msgs::msg::Control        | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
+| `~/input/steering"`   | autoware_vehicle_msgs::msg::SteeringReport | current status of steering used for steering feed back control                                                     |
+| `~/input/odometry`    | navigation_msgs::Odometry                  | twist topic in odometry is used.                                                                                   |
 
 ## Output topics
 
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp
@@ -20,6 +20,7 @@
 #include "autoware_raw_vehicle_cmd_converter/pid.hpp"
 #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -74,12 +75,13 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
 
   //!< @brief topic publisher for low level vehicle command
   rclcpp::Publisher<ActuationCommandStamped>::SharedPtr pub_actuation_cmd_;
-  //!< @brief subscriber for current velocity
-  rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
   //!< @brief subscriber for vehicle command
   rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
-  //!< @brief subscriber for steering
-  rclcpp::Subscription<Steering>::SharedPtr sub_steering_;
+  // polling subscribers
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_odometry_{
+    this, "~/input/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<Steering> sub_steering_{
+    this, "~/input/steering"};
 
   rclcpp::TimerBase::SharedPtr timer_;
 
@@ -109,9 +111,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
     const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero);
   double calculateBrakeMap(const double current_velocity, const double desired_acc);
   double calculateSteer(const double vel, const double steering, const double steer_rate);
-  void onSteering(const Steering::ConstSharedPtr msg);
   void onControlCmd(const Control::ConstSharedPtr msg);
-  void onVelocity(const Odometry::ConstSharedPtr msg);
   void publishActuationCmd();
   // for debugging
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_pub_steer_pid_;
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml
@@ -7,6 +7,7 @@
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="taiki.tanaka@tier4.jp">Tanaka Taiki</maintainer>
   <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
+  <maintainer email="sho.iwasawa.2@tier4.jp">Sho Iwasawa</maintainer>
 
   <license>Apache License 2.0</license>
 
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp
@@ -78,10 +78,6 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
   pub_actuation_cmd_ = create_publisher<ActuationCommandStamped>("~/output/actuation_cmd", 1);
   sub_control_cmd_ = create_subscription<Control>(
     "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1));
-  sub_velocity_ = create_subscription<Odometry>(
-    "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1));
-  sub_steering_ = create_subscription<Steering>(
-    "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1));
   debug_pub_steer_pid_ = create_publisher<Float32MultiArrayStamped>(
     "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1);
 
@@ -204,20 +200,18 @@ double RawVehicleCommandConverterNode::calculateBrakeMap(
   return desired_brake_cmd;
 }
 
-void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg)
-{
-  current_steer_ptr_ = std::make_unique<double>(msg->steering_tire_angle);
-}
-
-void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr msg)
-{
-  current_twist_ptr_ = std::make_unique<TwistStamped>();
-  current_twist_ptr_->header = msg->header;
-  current_twist_ptr_->twist = msg->twist.twist;
-}
-
 void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
 {
+  const auto odometry_msg = sub_odometry_.takeData();
+  const auto steering_msg = sub_steering_.takeData();
+  if (steering_msg) {
+    current_steer_ptr_ = std::make_unique<double>(steering_msg->steering_tire_angle);
+  }
+  if (odometry_msg) {
+    current_twist_ptr_ = std::make_unique<TwistStamped>();
+    current_twist_ptr_->header = odometry_msg->header;
+    current_twist_ptr_->twist = odometry_msg->twist.twist;
+  }
   control_cmd_ptr_ = msg;
   publishActuationCmd();
 }
