feat(raw_vehicle_cmd_converter): add vehicle adaptor (#8782)

* 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>

* feat(raw_vehicle_cmd_converter): publish vehicle adaptor output

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

* use control horizon

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

* revert carla

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

* update docs

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

---------

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

Author: kosuke55

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

vehicle/autoware_raw_vehicle_cmd_converter/README.md

@@ -45,6 +45,12 @@ vgr_coef_c: 0.042
 When `convert_steer_cmd_method: "vgr"` is selected, the node receives the control command from the controller as the desired tire angle and calculates the desired steering angle to output.
 Also, when `convert_actuation_to_steering_status: true`, this node receives the `actuation_status` topic and calculates the steer tire angle from the `steer_wheel_angle` and publishes it.
 
+### Vehicle Adaptor
+
+**Under development**
+A feature that compensates for control commands according to the dynamic characteristics of the vehicle.
+This feature works when `use_vehicle_adaptor: true` is set and requires `control_horizon` to be enabled, so you need to set `enable_control_cmd_horizon_pub: true` in the trajectory_follower node.
+
 ## Input topics
 
 | Name                       | Type                                       | Description                                                                                                                                                                                                                                                                                       |
@@ -54,6 +60,14 @@ Also, when `convert_actuation_to_steering_status: true`, this node receives the
 | `~/input/odometry`         | navigation_msgs::Odometry                  | twist topic in odometry is used.                                                                                                                                                                                                                                                                  |
 | `~/input/actuation_status` | tier4_vehicle_msgs::msg::ActuationStatus   | actuation status is assumed to receive the same type of status as sent to the vehicle side. For example, if throttle/brake pedal/steer_wheel_angle is sent, the same type of status is received. In the case of steer_wheel_angle, it is used to calculate steer_tire_angle and VGR in this node. |
 
+Input topics when vehicle_adaptor is enabled
+
+| Name                           | Type                                            | Description             |
+| ------------------------------ | ----------------------------------------------- | ----------------------- |
+| `~/input/accel`                | geometry_msgs::msg::AccelWithCovarianceStamped; | acceleration status     |
+| `~/input/operation_mode_state` | autoware_adapi_v1_msgs::msg::OperationModeState | operation mode status   |
+| `~/input/control_horizon`      | autoware_control_msgs::msg::ControlHorizon      | control horizon command |
+
 ## Output topics
 
 | Name                       | Type                                             | Description                                                                                                                          |

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:
@@ -79,24 +82,35 @@ 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_;
   rclcpp::Subscription<Steering>::SharedPtr sub_steering_;
   // polling subscribers
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_odometry_{
     this, "~/input/odometry"};
+  // polling subscribers for vehicle_adaptor
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> sub_accel_{
+    this, "~/input/accel"};
+  autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
+    this, "~/input/operation_mode_state"};
+  // control_horizon is an experimental topic, but vehicle_adaptor uses it to improve performance,
+  autoware::universe_utils::InterProcessPollingSubscriber<ControlHorizon> sub_control_horizon_{
+    this, "~/input/control_horizon"};
 
   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 +126,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

vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp

@@ -0,0 +1,50 @@
+//  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_control_msgs/msg/control_horizon.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_control_msgs::msg::ControlHorizon;
+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,
+    [[maybe_unused]] const ControlHorizon & control_horizon);
+
+private:
+};
+}  // namespace autoware::raw_vehicle_cmd_converter
+
+#endif  // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_

vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml

@@ -2,8 +2,11 @@
 <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="input_control_horizon" default="/control/trajectory_follower/controller_node_exe/debug/control_cmd_horizon"/>
   <arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
   <arg name="output_steering_status" default="/vehicle/status/steering_status"/>
   <!-- Parameter -->
@@ -13,8 +16,11 @@
     <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="~/input/control_horizon" to="$(var input_control_horizon)"/>
     <remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
     <remap from="~/output/steering_status" to="$(var output_steering_status)"/>
   </node>

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>

vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json

@@ -180,6 +180,11 @@
           "type": "boolean",
           "default": "true",
           "description": "convert actuation to steering status or not. Whether to subscribe to actuation_status and calculate and publish steering_status For example, receive the steering wheel angle and calculate the steering wheel angle based on the gear ratio. If false, the vehicle interface must publish steering_status."
+        },
+        "use_vehicle_adaptor": {
+          "type": "boolean",
+          "default": "false",
+          "description": "flag to enable feature that compensates control commands according to vehicle dynamics."
         }
       },
       "required": [

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.");
@@ -116,15 +118,21 @@ 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);
 }
 
 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 +144,34 @@ void RawVehicleCommandConverterNode::publishActuationCmd()
     }
   }
 
+  /* compensate control command if vehicle adaptor is enabled */
+  Control control_cmd = *control_cmd_ptr_;
+  if (use_vehicle_adaptor_) {
+    const auto current_accel = sub_accel_.takeData();
+    const auto current_operation_mode = sub_operation_mode_.takeData();
+    const auto control_horizon = sub_control_horizon_.takeData();
+    if (!current_accel || !current_operation_mode || !control_horizon) {
+      RCLCPP_WARN_EXPRESSION(
+        get_logger(), is_debugging_, "some pointers are null: %s, %s %s",
+        !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "",
+        !control_horizon ? "control_horizon" : "");
+      return;
+    }
+    control_cmd = vehicle_adaptor_.compensate(
+      *control_cmd_ptr_, *current_odometry_, *current_accel, *current_steer_ptr_,
+      *current_operation_mode, *control_horizon);
+    pub_compensated_control_cmd_->publish(control_cmd);
+  }
+
+  /* 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 +201,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 +280,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 +300,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);

vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp

@@ -0,0 +1,32 @@
+//  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,
+  [[maybe_unused]] const ControlHorizon & control_horizon)
+{
+  // 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