feat(raw_vehicle_cmd_converter): publish vehicle adaptor output

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp

@@ -82,6 +82,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
   //!< @brief topic publisher for low level vehicle command
   rclcpp::Publisher<ActuationCommandStamped>::SharedPtr pub_actuation_cmd_;
   rclcpp::Publisher<Steering>::SharedPtr pub_steering_status_;
+  rclcpp::Publisher<Control>::SharedPtr pub_compensated_control_cmd_;
   //!< @brief subscriber for vehicle command
   rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
   rclcpp::Subscription<ActuationStatusStamped>::SharedPtr sub_actuation_status_;

vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp

@@ -118,6 +118,11 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
   debug_pub_steer_pid_ = create_publisher<Float32MultiArrayStamped>(
     "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1);
 
+  if (use_vehicle_adaptor_) {
+    pub_compensated_control_cmd_ = create_publisher<Control>(
+      "/vehicle/raw_vehicle_cmd_converter/debug/compensated_control_cmd", 1);
+  }
+
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
 }
 
@@ -156,6 +161,9 @@ void RawVehicleCommandConverterNode::publishActuationCmd()
                                     *control_cmd_ptr_, *current_odometry_, *current_accel,
                                     *current_steer_ptr_, *current_operation_mode)
                                 : *control_cmd_ptr_;
+  if (use_vehicle_adaptor_) {
+    pub_compensated_control_cmd_->publish(control_cmd);
+  }
 
   /* calculate actuation command */
   double desired_accel_cmd = 0.0;