fix(autoware_carla_interface): improve lateral control tracking (#10312)

Convert steer angle to actuator command, model steering with first order dynamics, and update the steer_map.csv

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

Author: stevenbrills

simulator/autoware_carla_interface/calibration_maps/steer_map.csv

@@ -1,4 +1,12 @@
-default,-10,0,10
--1,-0.9,-0.9,-0.9
-0,0,0,0
-1,0.9,0.9,0.9
+default,-1,-0.8,-0.6,-0.4,-0.2,0,0.2,0.4,0.6,0.8,1
+-1,0,-0.404,-0.808,-1.212,-1.616,-2.02,-2.346,-2.672,-2.998,-3.324,-3.65
+-0.8,0.4,-0.00599999999999995,-0.412,-0.818,-1.224,-1.63,-2.024,-2.418,-2.812,-3.206,-3.6
+-0.6,0.805,0.394,-0.017,-0.428,-0.839,-1.25,-1.6466,-2.0432,-2.4398,-2.8364,-3.233
+-0.4,1.11,0.7164,0.3228,-0.0707999999999998,-0.4644,-0.858,-1.3164,-1.7748,-2.2332,-2.6916,-3.15
+-0.2,1.68,1.256,0.832,0.408,-0.0160000000000002,-0.44,-0.836,-1.232,-1.628,-2.024,-2.42
+0,2.2,1.76,1.32,0.88,0.44,0,-0.44,-0.88,-1.32,-1.76,-2.2
+0.2,2.42,2.024,1.628,1.232,0.836,0.44,0.016,-0.408,-0.832,-1.256,-1.68
+0.4,3.15,2.6916,2.2332,1.7748,1.3164,0.858,0.4644,0.0708,-0.3228,-0.7164,-1.11
+0.6,3.233,2.8364,2.4398,2.0432,1.6466,1.25,0.839,0.428,0.0169999999999999,-0.394,-0.805
+0.8,3.6,3.208,2.816,2.424,2.032,1.64,1.232,0.824,0.416,0.00799999999999979,-0.4
+1,3.66,3.332,3.004,2.676,2.348,2.02,1.616,1.212,0.808,0.404,0

simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py

@@ -50,6 +50,9 @@
 class carla_ros2_interface(object):
     def __init__(self):
         self.sensor_interface = SensorInterface()
+        self.prev_timestamp = None
+        self.prev_steer_output = 0.0
+        self.tau = 0.2
         self.timestamp = None
         self.ego_actor = None
         self.physics_control = None
@@ -382,6 +385,21 @@ def imu(self, carla_imu_measurement):
 
         self.pub_imu.publish(imu_msg)
 
+    def first_order_steering(self, steer_input):
+        """First order steering model."""
+        steer_output = 0.0
+        if self.prev_timestamp is None:
+            self.prev_timestamp = self.timestamp
+
+        dt = self.timestamp - self.prev_timestamp
+        if dt > 0.0:
+            steer_output = self.prev_steer_output + (steer_input - self.prev_steer_output) * (
+                dt / (self.tau + dt)
+            )
+        self.prev_steer_output = steer_output
+        self.prev_timestamp = self.timestamp
+        return steer_output
+
     def control_callback(self, in_cmd):
         """Convert and publish CARLA Ego Vehicle Control to AUTOWARE."""
         out_cmd = carla.VehicleControl()
@@ -392,11 +410,7 @@ def control_callback(self, in_cmd):
         max_steer_ratio = numpy.interp(
             abs(current_vel.x), [v.x for v in steer_curve], [v.y for v in steer_curve]
         )
-        out_cmd.steer = (
-            -in_cmd.actuation.steer_cmd
-            * max_steer_ratio
-            * math.radians(self.physics_control.wheels[0].max_steer_angle)
-        )
+        out_cmd.steer = self.first_order_steering(-in_cmd.actuation.steer_cmd) * max_steer_ratio
         out_cmd.brake = in_cmd.actuation.brake_cmd
         self.current_control = out_cmd