17
17
import math
18
18
import threading
19
19
20
- from autoware_auto_control_msgs .msg import AckermannControlCommand
21
20
from autoware_auto_vehicle_msgs .msg import ControlModeReport
22
21
from autoware_auto_vehicle_msgs .msg import GearReport
23
22
from autoware_auto_vehicle_msgs .msg import SteeringReport
37
36
from sensor_msgs .msg import PointCloud2
38
37
from sensor_msgs .msg import PointField
39
38
from std_msgs .msg import Header
39
+ from tier4_vehicle_msgs .msg import ActuationCommandStamped
40
+ from tier4_vehicle_msgs .msg import ActuationStatusStamped
40
41
from transforms3d .euler import euler2quat
41
42
42
43
from .modules .carla_data_provider import CarlaDataProvider
@@ -111,14 +112,14 @@ def setup(self):
111
112
112
113
# Subscribing Autoware Control messages and converting to CARLA control
113
114
self .sub_control = self .ros2_node .create_subscription (
114
- AckermannControlCommand , "/control/command/control_cmd " , self .control_callback , 1
115
+ ActuationCommandStamped , "/control/command/actuation_cmd " , self .control_callback , 1
115
116
)
116
117
117
118
self .sub_vehicle_initialpose = self .ros2_node .create_subscription (
118
119
PoseWithCovarianceStamped , "initialpose" , self .initialpose_callback , 1
119
120
)
120
121
121
- self .current_control = carla .VehicleAckermannControl ()
122
+ self .current_control = carla .VehicleControl ()
122
123
123
124
# Direct data publishing from CARLA for Autoware
124
125
self .pub_pose_with_cov = self .ros2_node .create_publisher (
@@ -139,6 +140,9 @@ def setup(self):
139
140
self .pub_gear_state = self .ros2_node .create_publisher (
140
141
GearReport , "/vehicle/status/gear_status" , 1
141
142
)
143
+ self .actuation_status = self .ros2_node .create_publisher (
144
+ ActuationStatusStamped , "/vehicle/status/actuation_status" , 1
145
+ )
142
146
143
147
# Create Publisher for each Physical Sensors
144
148
for sensor in self .sensors ["sensors" ]:
@@ -172,7 +176,6 @@ def __call__(self):
172
176
input_data = self .sensor_interface .get_data ()
173
177
timestamp = GameTime .get_time ()
174
178
control = self .run_step (input_data , timestamp )
175
- control .manual_gear_shift = False
176
179
return control
177
180
178
181
def get_param (self ):
@@ -377,17 +380,10 @@ def imu(self, carla_imu_measurement):
377
380
378
381
def control_callback (self , in_cmd ):
379
382
"""Convert and publish CARLA Ego Vehicle Control to AUTOWARE."""
380
- out_cmd = carla .VehicleAckermannControl (
381
- steer = numpy .clip (
382
- - math .degrees (in_cmd .lateral .steering_tire_angle ) / self .max_steering_angle ,
383
- - 1.0 ,
384
- 1.0 ,
385
- ),
386
- steer_speed = in_cmd .lateral .steering_tire_rotation_rate ,
387
- speed = in_cmd .longitudinal .speed ,
388
- acceleration = in_cmd .longitudinal .acceleration ,
389
- jerk = in_cmd .longitudinal .jerk ,
390
- )
383
+ out_cmd = carla .VehicleControl ()
384
+ out_cmd .throttle = in_cmd .actuation .accel_cmd
385
+ out_cmd .steer = - in_cmd .actuation .steer_cmd
386
+ out_cmd .brake = in_cmd .actuation .brake_cmd
391
387
self .current_control = out_cmd
392
388
393
389
def ego_status (self ):
@@ -398,6 +394,7 @@ def ego_status(self):
398
394
ego_vehicle = CarlaDataProvider .get_actor_by_name (
399
395
self .param_values ["ego_vehicle_role_name" ]
400
396
)
397
+ control = ego_vehicle .get_control ()
401
398
402
399
self .current_vel = CarlaDataProvider .get_velocity (
403
400
CarlaDataProvider .get_actor_by_name (self .param_values ["ego_vehicle_role_name" ])
@@ -408,6 +405,7 @@ def ego_status(self):
408
405
out_ctrl_mode = ControlModeReport ()
409
406
out_gear_state = GearReport ()
410
407
out_traffic = TrafficSignalArray ()
408
+ out_actuation_status = ActuationStatusStamped ()
411
409
412
410
out_vel_state .header = self .get_msg_header (frame_id = "base_link" )
413
411
out_vel_state .longitudinal_velocity = self .current_vel
@@ -425,6 +423,14 @@ def ego_status(self):
425
423
out_ctrl_mode .stamp = out_vel_state .header .stamp
426
424
out_ctrl_mode .mode = ControlModeReport .AUTONOMOUS
427
425
426
+ out_actuation_status .header = self .get_msg_header (frame_id = "base_link" )
427
+ out_actuation_status .status .accel_status = control .throttle
428
+ out_actuation_status .status .brake_status = control .brake
429
+ out_actuation_status .status .steer_status = - math .radians (
430
+ ego_vehicle .get_wheel_steer_angle (carla .VehicleWheelLocation .FL_Wheel )
431
+ )
432
+
433
+ self .actuation_status .publish (out_actuation_status )
428
434
self .pub_vel_state .publish (out_vel_state )
429
435
self .pub_steering_state .publish (out_steering_state )
430
436
self .pub_ctrl_mode .publish (out_ctrl_mode )
@@ -455,4 +461,4 @@ def run_step(self, input_data, timestamp):
455
461
456
462
# Publish ego vehicle status
457
463
self .ego_status ()
458
- return self .current_control
464
+ return self .current_control
0 commit comments