feat(raw_vehicle_cmd_converter): add steer command conversion with VGR (

autowarefoundation#8504) * feat(raw_vehicle_cmd_converter): add steer command conversion with VGR Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * make class and add test Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * remove member vgr_coef from node Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update readme Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add svg Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add plot scripts Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update vehicle/autoware_raw_vehicle_cmd_converter/README.md Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * not always subscribe actuation_status Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add comment for using normal sub for steering status Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
tier4 · Aug 22, 2024 · e178d3e · e178d3e
1 parent a363f9e
commit e178d3e
Show file tree

Hide file tree

Showing 12 changed files with 3,982 additions and 49 deletions.
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt
@@ -10,6 +10,7 @@ ament_auto_add_library(actuation_map_converter SHARED
   src/steer_map.cpp
   src/csv_loader.cpp
   src/pid.cpp
+  src/vgr.cpp
 )
 
 ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md
@@ -24,19 +24,42 @@ Once the acceleration map is crafted, it should be loaded when the RawVehicleCmd
 
 For ease of calibration and adjustments to the lookup table, an auto-calibration tool is available. More information and instructions for this tool can be found [here](https://github.com/autowarefoundation/autoware.universe/blob/main/vehicle/autoware_accel_brake_map_calibrator/README.md).
 
+### Variable Gear Ratio (VGR)
+
+This is a gear ratio for converting tire angle to steering angle. Generally, to improve operability, the gear ratio becomes dynamically larger as the speed increases or the steering angle becomes smaller. For a certain vehicle, data was acquired and the gear ratio was approximated by the following formula.
+
+$$
+a + b \times v^2 - c \times \lvert \delta \rvert
+$$
+
+For that vehicle, the coefficients were as follows.
+
+```yaml
+vgr_coef_a: 15.713
+vgr_coef_b: 0.053
+vgr_coef_c: 0.042
+```
+
+![vgr](./figure/vgr.svg)
+
+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.
+
 ## 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::msg::SteeringReport | current status of steering used for steering feed back control                                                     |
-| `~/input/odometry`    | 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 | subscribe only when `convert_actuation_to_steering_status: false`. current status of steering used for steering feed back control                                                                                                                                                                 |
+| `~/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. |
 
 ## Output topics
 
-| Name                     | Type                                             | Description                                             |
-| ------------------------ | ------------------------------------------------ | ------------------------------------------------------- |
-| `~/output/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input |
+| Name                       | Type                                             | Description                                                                                                                          |
+| -------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------ |
+| `~/output/actuation_cmd`   | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input                                                                              |
+| `~/output/steering_status` | autoware_vehicle_msgs::msg::SteeringReport       | publish only when `convert_actuation_to_steering_status: true`. steer tire angle is calculated from steer wheel angle and published. |
 
 ## Parameters
 

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml
@@ -26,3 +26,8 @@
       max_d: 0.0
       min_d: 0.0
       invalid_integration_decay: 0.97
+    convert_steer_cmd_method: "vgr"  # "vgr" or "steer_map"
+    vgr_coef_a: 15.713
+    vgr_coef_b: 0.053
+    vgr_coef_c: 0.042
+    convert_actuation_to_steering_status: true