change control to actuation

Author: mraditya01

simulator/carla_autoware/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

simulator/carla_autoware/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

simulator/carla_autoware/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

simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml

@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    csv_path_accel_map: $(find-pkg-share carla_autoware)/accel_map.csv
+    csv_path_brake_map: $(find-pkg-share carla_autoware)/brake_map.csv
+    csv_path_steer_map: $(find-pkg-share carla_autoware)/steer_map.csv
+    convert_accel_cmd: true
+    convert_brake_cmd: true
+    convert_steer_cmd: true
+    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

simulator/carla_autoware/launch/carla_autoware.launch.xml

@@ -29,7 +29,6 @@
   </node>
 
   <!-- Awsim configuration frame to CARLA frame -->
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="ego_vehicle2base_link" args="0 0 0 0 0 0 /ego_vehicle /base_link "/>
   <node pkg="tf2_ros" exec="static_transform_publisher" name="velodyne_top" args="0 0 1 -1.5386 -0.015 0.001 /velodyne_top /velodyne_top_changed "/>
   <node pkg="tf2_ros" exec="static_transform_publisher" name="imu" args="0 0 1 -3.10519265 -0.015 -3.14059265359 /tamagawa/imu_link /tamagawa/imu_link_changed "/>
   <node

simulator/carla_autoware/setup.py

@@ -12,7 +12,8 @@
     version="0.0.0",
     packages=[package_name],
     data_files=[
-        ("share/" + package_name, glob("config/objects.json")),
+        ("share/" + package_name, glob("config/*")),
+        ("share/" + package_name, glob("calibration_maps/*.csv")),
         ("share/" + package_name, ["package.xml"]),
         (os.path.join("share", package_name), glob("launch/carla_autoware.launch.xml")),
     ],
@@ -27,4 +28,4 @@
         "console_scripts": ["carla_autoware = carla_autoware.carla_autoware:main"],
     },
     package_dir={"": "src"},
-)
+)

simulator/carla_autoware/src/carla_autoware/carla_autoware.py

@@ -48,7 +48,7 @@ def _tick_sensor(self, timestamp):
                 ego_action = self.sensor()
             except SensorReceivedNoData as e:
                 raise RuntimeError(e)
-            self.ego_vehicle.apply_ackermann_control(ego_action)
+            self.ego_vehicle.apply_control(ego_action)
         if self.running:
             CarlaDataProvider.get_world().tick()
 
@@ -197,4 +197,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()

simulator/carla_autoware/src/carla_autoware/carla_ros.py

@@ -17,7 +17,6 @@
 import math
 import threading
 
-from autoware_auto_control_msgs.msg import AckermannControlCommand
 from autoware_auto_vehicle_msgs.msg import ControlModeReport
 from autoware_auto_vehicle_msgs.msg import GearReport
 from autoware_auto_vehicle_msgs.msg import SteeringReport
@@ -37,6 +36,8 @@
 from sensor_msgs.msg import PointCloud2
 from sensor_msgs.msg import PointField
 from std_msgs.msg import Header
+from tier4_vehicle_msgs.msg import ActuationCommandStamped
+from tier4_vehicle_msgs.msg import ActuationStatusStamped
 from transforms3d.euler import euler2quat
 
 from .modules.carla_data_provider import CarlaDataProvider
@@ -111,14 +112,14 @@ def setup(self):
 
         # Subscribing Autoware Control messages and converting to CARLA control
         self.sub_control = self.ros2_node.create_subscription(
-            AckermannControlCommand, "/control/command/control_cmd", self.control_callback, 1
+            ActuationCommandStamped, "/control/command/actuation_cmd", self.control_callback, 1
         )
 
         self.sub_vehicle_initialpose = self.ros2_node.create_subscription(
             PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1
         )
 
-        self.current_control = carla.VehicleAckermannControl()
+        self.current_control = carla.VehicleControl()
 
         # Direct data publishing from CARLA for Autoware
         self.pub_pose_with_cov = self.ros2_node.create_publisher(
@@ -139,6 +140,9 @@ def setup(self):
         self.pub_gear_state = self.ros2_node.create_publisher(
             GearReport, "/vehicle/status/gear_status", 1
         )
+        self.actuation_status = self.ros2_node.create_publisher(
+            ActuationStatusStamped, "/vehicle/status/actuation_status", 1
+        )
 
         # Create Publisher for each Physical Sensors
         for sensor in self.sensors["sensors"]:
@@ -172,7 +176,6 @@ def __call__(self):
         input_data = self.sensor_interface.get_data()
         timestamp = GameTime.get_time()
         control = self.run_step(input_data, timestamp)
-        control.manual_gear_shift = False
         return control
 
     def get_param(self):
@@ -377,17 +380,10 @@ def imu(self, carla_imu_measurement):
 
     def control_callback(self, in_cmd):
         """Convert and publish CARLA Ego Vehicle Control to AUTOWARE."""
-        out_cmd = carla.VehicleAckermannControl(
-            steer=numpy.clip(
-                -math.degrees(in_cmd.lateral.steering_tire_angle) / self.max_steering_angle,
-                -1.0,
-                1.0,
-            ),
-            steer_speed=in_cmd.lateral.steering_tire_rotation_rate,
-            speed=in_cmd.longitudinal.speed,
-            acceleration=in_cmd.longitudinal.acceleration,
-            jerk=in_cmd.longitudinal.jerk,
-        )
+        out_cmd = carla.VehicleControl()
+        out_cmd.throttle = in_cmd.actuation.accel_cmd
+        out_cmd.steer = -in_cmd.actuation.steer_cmd
+        out_cmd.brake = in_cmd.actuation.brake_cmd
         self.current_control = out_cmd
 
     def ego_status(self):
@@ -398,6 +394,7 @@ def ego_status(self):
         ego_vehicle = CarlaDataProvider.get_actor_by_name(
             self.param_values["ego_vehicle_role_name"]
         )
+        control = ego_vehicle.get_control()
 
         self.current_vel = CarlaDataProvider.get_velocity(
             CarlaDataProvider.get_actor_by_name(self.param_values["ego_vehicle_role_name"])
@@ -408,6 +405,7 @@ def ego_status(self):
         out_ctrl_mode = ControlModeReport()
         out_gear_state = GearReport()
         out_traffic = TrafficSignalArray()
+        out_actuation_status = ActuationStatusStamped()
 
         out_vel_state.header = self.get_msg_header(frame_id="base_link")
         out_vel_state.longitudinal_velocity = self.current_vel
@@ -425,6 +423,14 @@ def ego_status(self):
         out_ctrl_mode.stamp = out_vel_state.header.stamp
         out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS
 
+        out_actuation_status.header = self.get_msg_header(frame_id="base_link")
+        out_actuation_status.status.accel_status = control.throttle
+        out_actuation_status.status.brake_status = control.brake
+        out_actuation_status.status.steer_status = -math.radians(
+            ego_vehicle.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel)
+        )
+
+        self.actuation_status.publish(out_actuation_status)
         self.pub_vel_state.publish(out_vel_state)
         self.pub_steering_state.publish(out_steering_state)
         self.pub_ctrl_mode.publish(out_ctrl_mode)
@@ -455,4 +461,4 @@ def run_step(self, input_data, timestamp):
 
         # Publish ego vehicle status
         self.ego_status()
-        return self.current_control
+        return self.current_control