feat(raw_vehicle_cmd_converter): add vehicle adaptor

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

sub operation status

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

Author: kosuke55

Diff for: simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml

@@ -31,6 +31,7 @@
 
     <arg name="input_control_cmd" default="/control/command/control_cmd"/>
     <arg name="input_odometry" default="/localization/kinematic_state"/>
+    <arg name="input_accel" default="/localization/acceleration"/>
     <arg name="input_steering" default="/vehicle/status/steering_status"/>
     <arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
     <arg name="config_file" default="$(find-pkg-share autoware_carla_interface)/raw_vehicle_cmd_converter.param.yaml"/>
@@ -39,6 +40,7 @@
       <param from="$(var config_file)" allow_substs="true"/>
       <remap from="~/input/control_cmd" to="$(var input_control_cmd)"/>
       <remap from="~/input/odometry" to="$(var input_odometry)"/>
+      <remap from="~/input/accel" to="$(var input_accel)"/>
       <remap from="~/input/steering" to="$(var input_steering)"/>
       <remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
     </node>

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt

@@ -13,11 +13,15 @@ ament_auto_add_library(actuation_map_converter SHARED
   src/vgr.cpp
 )
 
+ament_auto_add_library(vehicle_adaptor SHARED
+  src/vehicle_adaptor/vehicle_adaptor.cpp
+)
+
 ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED
   src/node.cpp
 )
 
-target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter)
+target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter vehicle_adaptor)
 
 rclcpp_components_register_node(autoware_raw_vehicle_cmd_converter_node_component
   PLUGIN "autoware::raw_vehicle_cmd_converter::RawVehicleCommandConverterNode"
@@ -30,7 +34,7 @@ if(BUILD_TESTING)
   )
   set(TEST_RAW_VEHICLE_CMD_CONVERTER_EXE test_autoware_raw_vehicle_cmd_converter)
   ament_add_ros_isolated_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES})
-  target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter autoware_raw_vehicle_cmd_converter_node_component)
+  target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter vehicle_adaptor autoware_raw_vehicle_cmd_converter_node_component)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp

@@ -21,10 +21,12 @@
 #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp"
 #include "autoware_raw_vehicle_cmd_converter/pid.hpp"
 #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp"
+#include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp"
 #include "autoware_raw_vehicle_cmd_converter/vgr.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/steering_report.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
@@ -46,7 +48,8 @@ using tier4_vehicle_msgs::msg::ActuationStatusStamped;
 using TwistStamped = geometry_msgs::msg::TwistStamped;
 using Odometry = nav_msgs::msg::Odometry;
 using Steering = autoware_vehicle_msgs::msg::SteeringReport;
-
+using autoware_adapi_v1_msgs::msg::OperationModeState;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 class DebugValues
 {
 public:
@@ -86,17 +89,23 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
   // polling subscribers
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_odometry_{
     this, "~/input/odometry"};
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> sub_accel_{
+    this, "~/input/accel"};
+  autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
+    this, "~/input/operation_mode_state"};
 
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::unique_ptr<TwistStamped> current_twist_ptr_;  // [m/s]
   std::unique_ptr<double> current_steer_ptr_;
   ActuationStatusStamped::ConstSharedPtr actuation_status_ptr_;
+  Odometry::ConstSharedPtr current_odometry_;
   Control::ConstSharedPtr control_cmd_ptr_;
   AccelMap accel_map_;
   BrakeMap brake_map_;
   SteerMap steer_map_;
   VGR vgr_;
+  VehicleAdaptor vehicle_adaptor_;
   // TODO(tanaka): consider accel/brake pid too
   PIDController steer_pid_;
   bool ff_map_initialized_;
@@ -112,6 +121,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
   bool convert_brake_cmd_;                                             //!< @brief use brake or not
   std::optional<std::string> convert_steer_cmd_method_{std::nullopt};  //!< @brief method to convert
   bool need_to_subscribe_actuation_status_{false};
+  bool use_vehicle_adaptor_{false};
   rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME};
 
   // Whether to subscribe to actuation_status and calculate and publish steering_status

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp

@@ -0,0 +1,47 @@
+//  Copyright 2024 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.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#ifndef AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_
+#define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_vehicle_msgs/msg/steering_report.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace autoware::raw_vehicle_cmd_converter
+{
+
+using autoware_adapi_v1_msgs::msg::OperationModeState;
+using autoware_control_msgs::msg::Control;
+using autoware_vehicle_msgs::msg::SteeringReport;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using nav_msgs::msg::Odometry;
+
+class VehicleAdaptor
+{
+public:
+  VehicleAdaptor() = default;
+  Control compensate(
+    const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry,
+    [[maybe_unused]] const AccelWithCovarianceStamped & accel,
+    [[maybe_unused]] const double steering,
+    [[maybe_unused]] const OperationModeState & operation_mode);
+
+private:
+};
+}  // namespace autoware::raw_vehicle_cmd_converter
+
+#endif  // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml

@@ -2,8 +2,10 @@
 <launch>
   <arg name="input_control_cmd" default="/control/command/control_cmd"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
+  <arg name="input_accel" default="/localization/acceleration"/>
   <arg name="input_steering" default="/vehicle/status/steering_status"/>
   <arg name="input_actuation_status" default="/vehicle/status/actuation_status"/>
+  <arg name="input_operation_mode_state" default="/system/operation_mode/state"/>
   <arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
   <arg name="output_steering_status" default="/vehicle/status/steering_status"/>
   <!-- Parameter -->
@@ -13,8 +15,10 @@
     <param from="$(var config_file)" allow_substs="true"/>
     <remap from="~/input/control_cmd" to="$(var input_control_cmd)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/input/accel" to="$(var input_accel)"/>
     <remap from="~/input/steering" to="$(var input_steering)"/>
     <remap from="~/input/actuation_status" to="$(var input_actuation_status)"/>
+    <remap from="~/input/operation_mode_state" to="$(var input_operation_mode_state)"/>
     <remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
     <remap from="~/output/steering_status" to="$(var output_steering_status)"/>
   </node>

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_control_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_vehicle_msgs</depend>

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp

@@ -40,6 +40,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
   // for steering steer controller
   use_steer_ff_ = declare_parameter<bool>("use_steer_ff");
   use_steer_fb_ = declare_parameter<bool>("use_steer_fb");
+  use_vehicle_adaptor_ = declare_parameter<bool>("use_vehicle_adaptor", false);
+
   if (convert_accel_cmd_) {
     if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) {
       throw std::invalid_argument("Accel map is invalid.");
@@ -121,10 +123,11 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
 
 void RawVehicleCommandConverterNode::publishActuationCmd()
 {
-  if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) {
+  /* check if all necessary data is available */
+  if (!current_odometry_ || !control_cmd_ptr_ || !current_steer_ptr_) {
     RCLCPP_WARN_EXPRESSION(
       get_logger(), is_debugging_, "some pointers are null: %s, %s, %s",
-      !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "",
+      !current_odometry_ ? "odometry" : "", !control_cmd_ptr_ ? "cmd" : "",
       !current_steer_ptr_ ? "steer" : "");
     return;
   }
@@ -136,14 +139,33 @@ void RawVehicleCommandConverterNode::publishActuationCmd()
     }
   }
 
+  const auto current_accel = sub_accel_.takeData();
+  const auto current_operation_mode = sub_operation_mode_.takeData();
+  if (use_vehicle_adaptor_) {
+    if (!current_accel || !current_operation_mode) {
+      RCLCPP_WARN_EXPRESSION(
+        get_logger(), is_debugging_, "some pointers are null: %s, %s",
+        !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "");
+      return;
+    }
+  }
+
+  /* compensate control command if vehicle adaptor is enabled */
+  const Control control_cmd = use_vehicle_adaptor_
+                                ? vehicle_adaptor_.compensate(
+                                    *control_cmd_ptr_, *current_odometry_, *current_accel,
+                                    *current_steer_ptr_, *current_operation_mode)
+                                : *control_cmd_ptr_;
+
+  /* calculate actuation command */
   double desired_accel_cmd = 0.0;
   double desired_brake_cmd = 0.0;
   double desired_steer_cmd = 0.0;
   ActuationCommandStamped actuation_cmd;
-  const double acc = control_cmd_ptr_->longitudinal.acceleration;
-  const double vel = current_twist_ptr_->twist.linear.x;
-  const double steer = control_cmd_ptr_->lateral.steering_tire_angle;
-  const double steer_rate = control_cmd_ptr_->lateral.steering_tire_rotation_rate;
+  const double acc = control_cmd.longitudinal.acceleration;
+  const double vel = current_odometry_->twist.twist.linear.x;
+  const double steer = control_cmd.lateral.steering_tire_angle;
+  const double steer_rate = control_cmd.lateral.steering_tire_rotation_rate;
   bool accel_cmd_is_zero = true;
   if (convert_accel_cmd_) {
     desired_accel_cmd = calculateAccelMap(vel, acc, accel_cmd_is_zero);
@@ -173,7 +195,7 @@ void RawVehicleCommandConverterNode::publishActuationCmd()
     desired_steer_cmd = calculateSteerFromMap(vel, steer, steer_rate);
   }
   actuation_cmd.header.frame_id = "base_link";
-  actuation_cmd.header.stamp = control_cmd_ptr_->stamp;
+  actuation_cmd.header.stamp = control_cmd.stamp;
   actuation_cmd.actuation.accel_cmd = desired_accel_cmd;
   actuation_cmd.actuation.brake_cmd = desired_brake_cmd;
   actuation_cmd.actuation.steer_cmd = desired_steer_cmd;
@@ -252,12 +274,7 @@ double RawVehicleCommandConverterNode::calculateBrakeMap(
 
 void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
 {
-  const auto odometry_msg = sub_odometry_.takeData();
-  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;
-  }
+  current_odometry_ = sub_odometry_.takeData();
   control_cmd_ptr_ = msg;
   publishActuationCmd();
 }
@@ -277,14 +294,11 @@ void RawVehicleCommandConverterNode::onActuationStatus(
   }
 
   // calculate steering status from actuation status
-  const auto odometry_msg = sub_odometry_.takeData();
-  if (odometry_msg) {
+  current_odometry_ = sub_odometry_.takeData();
+  if (current_odometry_) {
     if (convert_steer_cmd_method_.value() == "vgr") {
-      current_twist_ptr_ = std::make_unique<TwistStamped>();
-      current_twist_ptr_->header = odometry_msg->header;
-      current_twist_ptr_->twist = odometry_msg->twist.twist;
       current_steer_ptr_ = std::make_unique<double>(vgr_.calculateSteeringTireState(
-        current_twist_ptr_->twist.linear.x, actuation_status_ptr_->status.steer_status));
+        current_odometry_->twist.twist.linear.x, actuation_status_ptr_->status.steer_status));
       Steering steering_msg{};
       steering_msg.steering_tire_angle = *current_steer_ptr_;
       pub_steering_status_->publish(steering_msg);

Diff for: vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp

@@ -0,0 +1,31 @@
+//  Copyright 2024 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.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp"
+
+#include <iostream>
+
+namespace autoware::raw_vehicle_cmd_converter
+{
+Control VehicleAdaptor::compensate(
+  const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry,
+  [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering,
+  [[maybe_unused]] const OperationModeState & operation_mode)
+{
+  // TODO(someone): implement the control command compensation
+  Control output_control_cmd = input_control_cmd;
+  std::cerr << "vehicle adaptor: compensate control command" << std::endl;
+  return output_control_cmd;
+}
+}  // namespace autoware::raw_vehicle_cmd_converter