diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md
index 25e816e6f8692..858db3cbaa768 100644
--- 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
index 9b64c26770e40..26b3dde029ebc 100644
--- 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
index 25a329be3a6b9..973af0dc112fb 100644
--- 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
index 044d698e8b9fc..0051dea0a5103 100644
--- 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();
}