From f61b68196ef1327ba873de9e66da66ef2aacde5d Mon Sep 17 00:00:00 2001
From: Leann Hsu <leann9001@gmail.com>
Date: Mon, 17 Feb 2025 16:17:29 +0800
Subject: [PATCH 1/2] Switch vehicle control from Ackermann to actuation

---
 config/zenoh-bridge-ros2dds-conf.json5        |  2 ++
 external/zenoh_carla_bridge                   |  2 +-
 src/autoware_carla_launch/CMakeLists.txt      | 10 +++++++
 src/autoware_carla_launch/README.md           | 12 ++++++++
 .../calibration_maps/accel_map.csv            |  7 +++++
 .../calibration_maps/brake_map.csv            | 10 +++++++
 .../calibration_maps/steer_map.csv            |  4 +++
 .../raw_vehicle_cmd_converter.param.yaml      | 28 +++++++++++++++++++
 .../launch/autoware_zenoh.launch.xml          | 16 ++++++++++-
 9 files changed, 89 insertions(+), 2 deletions(-)
 create mode 100644 src/autoware_carla_launch/README.md
 create mode 100644 src/autoware_carla_launch/calibration_maps/accel_map.csv
 create mode 100644 src/autoware_carla_launch/calibration_maps/brake_map.csv
 create mode 100644 src/autoware_carla_launch/calibration_maps/steer_map.csv
 create mode 100644 src/autoware_carla_launch/config/raw_vehicle_cmd_converter.param.yaml

diff --git a/config/zenoh-bridge-ros2dds-conf.json5 b/config/zenoh-bridge-ros2dds-conf.json5
index 1d1c731..727f8ed 100644
--- a/config/zenoh-bridge-ros2dds-conf.json5
+++ b/config/zenoh-bridge-ros2dds-conf.json5
@@ -18,6 +18,7 @@
       allow: {
         publishers: [
             // Send to bridge
+            "/control/command/actuation_cmd",
             "/control/command/control_cmd",
             "/control/command/gear_cmd",
             "/control/current_gate_mode",
@@ -32,6 +33,7 @@
         ],
         subscribers: [
             // Receive from bridge
+            "/vehicle/status/actuation_status",
             "/vehicle/status/velocity_status",
             "/vehicle/status/steering_status",
             "/vehicle/status/control_mode",
diff --git a/external/zenoh_carla_bridge b/external/zenoh_carla_bridge
index c6eecf2..58279b7 160000
--- a/external/zenoh_carla_bridge
+++ b/external/zenoh_carla_bridge
@@ -1 +1 @@
-Subproject commit c6eecf2aa783d4f9191e4c5e0b546ee652a4776a
+Subproject commit 58279b743aca0e599036294461e91efcaeeb2997
diff --git a/src/autoware_carla_launch/CMakeLists.txt b/src/autoware_carla_launch/CMakeLists.txt
index 9e9e3f2..f512b84 100644
--- a/src/autoware_carla_launch/CMakeLists.txt
+++ b/src/autoware_carla_launch/CMakeLists.txt
@@ -32,4 +32,14 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(DIRECTORY
+  calibration_maps
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_package()
diff --git a/src/autoware_carla_launch/README.md b/src/autoware_carla_launch/README.md
new file mode 100644
index 0000000..c022523
--- /dev/null
+++ b/src/autoware_carla_launch/README.md
@@ -0,0 +1,12 @@
+# autoware_carla_launch
+
+Calibration maps and configuration file are sourced from [`autoware_carla_interface`](https://github.com/autowarefoundation/autoware.universe/tree/0.41.1/simulator/autoware_carla_interface) due to the same vehicle model.
+
+## Calibration Maps
+Calibration maps for acceleration, braking, and steering commands from Autoware to CARLA vehicle inputs.
+- `calibration_maps/accel_map.csv`
+- `calibration_maps/brake_map.csv`
+- `calibration_maps/steer_map.csv`
+
+## Configuration File
+- **`config/raw_vehicle_cmd_converter.param.yaml`**: Configures `autoware_raw_vehicle_cmd_converter_node`, specifying calibration maps and parameters for vehicle command translation.
diff --git a/src/autoware_carla_launch/calibration_maps/accel_map.csv b/src/autoware_carla_launch/calibration_maps/accel_map.csv
new file mode 100644
index 0000000..18718c3
--- /dev/null
+++ b/src/autoware_carla_launch/calibration_maps/accel_map.csv
@@ -0,0 +1,7 @@
+default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
+0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500
+0.100,0.167,0.166,-0.093,-0.243,-0.244,-0.245,-0.246,-0.247,-0.248,-0.249,-0.280
+0.200,0.941,0.464,0.186,0.004,-0.100,-0.101,-0.102,-0.103,-0.104,-0.105,-0.106
+0.300,1.747,1.332,0.779,0.778,0.777,0.776,0.775,0.774,0.720,0.640,0.580
+0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100
+0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610
diff --git a/src/autoware_carla_launch/calibration_maps/brake_map.csv b/src/autoware_carla_launch/calibration_maps/brake_map.csv
new file mode 100644
index 0000000..62b18b4
--- /dev/null
+++ b/src/autoware_carla_launch/calibration_maps/brake_map.csv
@@ -0,0 +1,10 @@
+default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
+0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500
+0.100,0.089,-0.226,-0.535,-0.536,-0.537,-0.538,-0.539,-0.540,-0.541,-0.542,-0.543
+0.200,-0.380,-0.414,-0.746,-0.800,-0.820,-0.850,-0.870,-0.890,-0.910,-0.940,-0.960
+0.300,-1.000,-1.040,-1.480,-1.550,-1.570,-1.590,-1.610,-1.630,-1.631,-1.632,-1.633
+0.400,-1.480,-1.500,-1.850,-2.050,-2.100,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106
+0.500,-1.490,-1.510,-1.860,-2.060,-2.110,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116
+0.600,-1.500,-1.520,-1.870,-2.070,-2.120,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126
+0.700,-1.510,-1.530,-1.880,-2.080,-2.130,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136
+0.800,-2.180,-2.200,-2.700,-2.800,-2.900,-2.950,-2.951,-2.952,-2.953,-2.954,-2.955
diff --git a/src/autoware_carla_launch/calibration_maps/steer_map.csv b/src/autoware_carla_launch/calibration_maps/steer_map.csv
new file mode 100644
index 0000000..077efb9
--- /dev/null
+++ b/src/autoware_carla_launch/calibration_maps/steer_map.csv
@@ -0,0 +1,4 @@
+default,-10,0,10
+-1,-0.9,-0.9,-0.9
+0,0,0,0
+1,0.9,0.9,0.9
diff --git a/src/autoware_carla_launch/config/raw_vehicle_cmd_converter.param.yaml b/src/autoware_carla_launch/config/raw_vehicle_cmd_converter.param.yaml
new file mode 100644
index 0000000..9523980
--- /dev/null
+++ b/src/autoware_carla_launch/config/raw_vehicle_cmd_converter.param.yaml
@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    csv_path_accel_map: $(find-pkg-share autoware_carla_launch)/calibration_maps/accel_map.csv
+    csv_path_brake_map: $(find-pkg-share autoware_carla_launch)/calibration_maps/brake_map.csv
+    csv_path_steer_map: $(find-pkg-share autoware_carla_launch)/calibration_maps/steer_map.csv
+    convert_accel_cmd: true
+    convert_brake_cmd: true
+    convert_steer_cmd: false
+    use_steer_ff: true
+    use_steer_fb: true
+    is_debugging: false
+    max_throttle: 0.4
+    max_brake: 0.8
+    max_steer: 1.0
+    min_steer: -1.0
+    steer_pid:
+      kp: 150.0
+      ki: 15.0
+      kd: 0.0
+      max: 8.0
+      min: -8.0
+      max_p: 8.0
+      min_p: -8.0
+      max_i: 8.0
+      min_i: -8.0
+      max_d: 0.0
+      min_d: 0.0
+      invalid_integration_decay: 0.97
\ No newline at end of file
diff --git a/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml b/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
index 1485449..ccc48b3 100644
--- a/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
+++ b/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
@@ -70,7 +70,21 @@
 
   <!-- CARLA -->
   <group if="$(var carla_interface)">
-		<node pkg="carla_pointcloud" exec="carla_pointcloud_node" name="carla_pointcloud_interface" output="screen"/>
+    <node pkg="carla_pointcloud" exec="carla_pointcloud_node" name="carla_pointcloud_interface" output="screen"/>
+
+    <!-- Converts Autoware's Ackermann control command into actuation command for CARLA vehicles -->
+    <arg name="input_control_cmd" default="/control/command/control_cmd"/>
+    <arg name="input_odometry" default="/localization/kinematic_state"/>
+    <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_launch)/config/raw_vehicle_cmd_converter.param.yaml"/>
+    <node pkg="autoware_raw_vehicle_cmd_converter" exec="autoware_raw_vehicle_cmd_converter_node" name="autoware_raw_vehicle_cmd_converter" output="screen">
+      <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/steering" to="$(var input_steering)"/>
+      <remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
+    </node>
 	</group>
 
   <!-- Relay for manual control -->

From 1b299866ab4de9ca2b4955795082d08d5e1702fd Mon Sep 17 00:00:00 2001
From: Leann Hsu <leann9001@gmail.com>
Date: Mon, 17 Feb 2025 16:28:54 +0800
Subject: [PATCH 2/2] Formatting code

---
 src/autoware_carla_launch/README.md                        | 5 ++++-
 src/autoware_carla_launch/launch/autoware_zenoh.launch.xml | 6 +++---
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/autoware_carla_launch/README.md b/src/autoware_carla_launch/README.md
index c022523..e504d89 100644
--- a/src/autoware_carla_launch/README.md
+++ b/src/autoware_carla_launch/README.md
@@ -3,10 +3,13 @@
 Calibration maps and configuration file are sourced from [`autoware_carla_interface`](https://github.com/autowarefoundation/autoware.universe/tree/0.41.1/simulator/autoware_carla_interface) due to the same vehicle model.
 
 ## Calibration Maps
+
 Calibration maps for acceleration, braking, and steering commands from Autoware to CARLA vehicle inputs.
+
 - `calibration_maps/accel_map.csv`
 - `calibration_maps/brake_map.csv`
 - `calibration_maps/steer_map.csv`
 
 ## Configuration File
-- **`config/raw_vehicle_cmd_converter.param.yaml`**: Configures `autoware_raw_vehicle_cmd_converter_node`, specifying calibration maps and parameters for vehicle command translation.
+
+- `config/raw_vehicle_cmd_converter.param.yaml`: Configures `autoware_raw_vehicle_cmd_converter_node`, specifying calibration maps and parameters for vehicle command translation.
diff --git a/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml b/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
index ccc48b3..12fee4f 100644
--- a/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
+++ b/src/autoware_carla_launch/launch/autoware_zenoh.launch.xml
@@ -32,7 +32,7 @@
   <arg name="launch_deprecated_api" default="true" description="launch deprecated API"/>
   <!-- Manual Controll -->
   <arg name="manual_control" default="true" description="Enable topic relay for manual control"/>
-  
+
   <group scoped="false">
     <include file="$(find-pkg-share autoware_launch)/launch/autoware.launch.xml">
       <!-- Common -->
@@ -89,7 +89,7 @@
 
   <!-- Relay for manual control -->
   <group if="$(var manual_control)">
-	  <node pkg="topic_tools" exec="relay" name="relay_control_cmd" args="external/selected/control_cmd control/command/control_cmd" output="screen"/>
-	  <node pkg="topic_tools" exec="relay" name="relay_gear_cmd" args="external/selected/gear_cmd control/command/gear_cmd" output="screen"/>
+    <node pkg="topic_tools" exec="relay" name="relay_control_cmd" args="external/selected/control_cmd control/command/control_cmd" output="screen"/>
+    <node pkg="topic_tools" exec="relay" name="relay_gear_cmd" args="external/selected/gear_cmd control/command/gear_cmd" output="screen"/>
   </group>
 </launch>