feat(raw_vehicle_cmd_converter): use polling subscriber

vehicle/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
 

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp

@@ -20,6 +20,7 @@
 #include "raw_vehicle_cmd_converter/pid.hpp"
 #include "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,14 @@ 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 +112,9 @@ 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 processSteering(const Steering::ConstSharedPtr msg);
   void onControlCmd(const Control::ConstSharedPtr msg);
-  void onVelocity(const Odometry::ConstSharedPtr msg);
+  void processOdometry(const Odometry::ConstSharedPtr msg);
   void publishActuationCmd();
   // for debugging
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_pub_steer_pid_;

vehicle/raw_vehicle_cmd_converter/src/node.cpp

@@ -1,4 +1,4 @@
 //  Copyright 2021 Tier IV, Inc. All rights reserved.
 //
 //  Licensed under the Apache License, Version 2.0 (the "License");
 //  you may not use this file except in compliance with the License.
@@ -78,10 +78,6 @@
   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,30 @@
   return desired_brake_cmd;
 }
 
-void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg)
+void RawVehicleCommandConverterNode::processSteering(const Steering::ConstSharedPtr msg)
 {
+  if (!msg) {
+    return;
+  }
   current_steer_ptr_ = std::make_unique<double>(msg->steering_tire_angle);
 }
 
-void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr msg)
+void RawVehicleCommandConverterNode::processOdometry(const Odometry::ConstSharedPtr msg)
 {
+  if (!msg) {
+    return;
+  }
   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();
+  processOdometry(odometry_msg);
+  processSteering(steering_msg);
   control_cmd_ptr_ = msg;
   publishActuationCmd();
 }