Skip to content

Commit 977cd70

Browse files
committed
change control to actuation
1 parent a9cfcb9 commit 977cd70

File tree

8 files changed

+76
-21
lines changed

8 files changed

+76
-21
lines changed
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
2+
0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500
3+
0.100,0.167,0.166,-0.093,-0.243,-0.244,-0.245,-0.246,-0.247,-0.248,-0.249,-0.280
4+
0.200,0.941,0.464,0.186,0.004,-0.100,-0.101,-0.102,-0.103,-0.104,-0.105,-0.106
5+
0.300,1.747,1.332,0.779,0.778,0.777,0.776,0.775,0.774,0.720,0.640,0.580
6+
0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100
7+
0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
2+
0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500
3+
0.100,0.089,-0.226,-0.535,-0.536,-0.537,-0.538,-0.539,-0.540,-0.541,-0.542,-0.543
4+
0.200,-0.380,-0.414,-0.746,-0.800,-0.820,-0.850,-0.870,-0.890,-0.910,-0.940,-0.960
5+
0.300,-1.000,-1.040,-1.480,-1.550,-1.570,-1.590,-1.610,-1.630,-1.631,-1.632,-1.633
6+
0.400,-1.480,-1.500,-1.850,-2.050,-2.100,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106
7+
0.500,-1.490,-1.510,-1.860,-2.060,-2.110,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116
8+
0.600,-1.500,-1.520,-1.870,-2.070,-2.120,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126
9+
0.700,-1.510,-1.530,-1.880,-2.080,-2.130,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136
10+
0.800,-2.180,-2.200,-2.700,-2.800,-2.900,-2.950,-2.951,-2.952,-2.953,-2.954,-2.955
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
default,-10,0,10
2+
-1,-0.9,-0.9,-0.9
3+
0,0,0,0
4+
1,0.9,0.9,0.9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
/**:
2+
ros__parameters:
3+
csv_path_accel_map: $(find-pkg-share carla_autoware)/accel_map.csv
4+
csv_path_brake_map: $(find-pkg-share carla_autoware)/brake_map.csv
5+
csv_path_steer_map: $(find-pkg-share carla_autoware)/steer_map.csv
6+
convert_accel_cmd: true
7+
convert_brake_cmd: true
8+
convert_steer_cmd: true
9+
use_steer_ff: true
10+
use_steer_fb: true
11+
is_debugging: false
12+
max_throttle: 0.4
13+
max_brake: 0.8
14+
max_steer: 1.0
15+
min_steer: -1.0
16+
steer_pid:
17+
kp: 150.0
18+
ki: 15.0
19+
kd: 0.0
20+
max: 8.0
21+
min: -8.0
22+
max_p: 8.0
23+
min_p: -8.0
24+
max_i: 8.0
25+
min_i: -8.0
26+
max_d: 0.0
27+
min_d: 0.0
28+
invalid_integration_decay: 0.97

simulator/carla_autoware/launch/carla_autoware.launch.xml

-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,6 @@
2929
</node>
3030

3131
<!-- Awsim configuration frame to CARLA frame -->
32-
<node pkg="tf2_ros" exec="static_transform_publisher" name="ego_vehicle2base_link" args="0 0 0 0 0 0 /ego_vehicle /base_link "/>
3332
<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 "/>
3433
<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 "/>
3534
<node

simulator/carla_autoware/setup.py

+3-2
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,8 @@
1212
version="0.0.0",
1313
packages=[package_name],
1414
data_files=[
15-
("share/" + package_name, glob("config/objects.json")),
15+
("share/" + package_name, glob("config/*")),
16+
("share/" + package_name, glob("calibration_maps/*.csv")),
1617
("share/" + package_name, ["package.xml"]),
1718
(os.path.join("share", package_name), glob("launch/carla_autoware.launch.xml")),
1819
],
@@ -27,4 +28,4 @@
2728
"console_scripts": ["carla_autoware = carla_autoware.carla_autoware:main"],
2829
},
2930
package_dir={"": "src"},
30-
)
31+
)

simulator/carla_autoware/src/carla_autoware/carla_autoware.py

+2-2
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ def _tick_sensor(self, timestamp):
4848
ego_action = self.sensor()
4949
except SensorReceivedNoData as e:
5050
raise RuntimeError(e)
51-
self.ego_vehicle.apply_ackermann_control(ego_action)
51+
self.ego_vehicle.apply_control(ego_action)
5252
if self.running:
5353
CarlaDataProvider.get_world().tick()
5454

@@ -197,4 +197,4 @@ def main():
197197

198198

199199
if __name__ == "__main__":
200-
main()
200+
main()

simulator/carla_autoware/src/carla_autoware/carla_ros.py

+22-16
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,6 @@
1717
import math
1818
import threading
1919

20-
from autoware_auto_control_msgs.msg import AckermannControlCommand
2120
from autoware_auto_vehicle_msgs.msg import ControlModeReport
2221
from autoware_auto_vehicle_msgs.msg import GearReport
2322
from autoware_auto_vehicle_msgs.msg import SteeringReport
@@ -37,6 +36,8 @@
3736
from sensor_msgs.msg import PointCloud2
3837
from sensor_msgs.msg import PointField
3938
from std_msgs.msg import Header
39+
from tier4_vehicle_msgs.msg import ActuationCommandStamped
40+
from tier4_vehicle_msgs.msg import ActuationStatusStamped
4041
from transforms3d.euler import euler2quat
4142

4243
from .modules.carla_data_provider import CarlaDataProvider
@@ -111,14 +112,14 @@ def setup(self):
111112

112113
# Subscribing Autoware Control messages and converting to CARLA control
113114
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
115116
)
116117

117118
self.sub_vehicle_initialpose = self.ros2_node.create_subscription(
118119
PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1
119120
)
120121

121-
self.current_control = carla.VehicleAckermannControl()
122+
self.current_control = carla.VehicleControl()
122123

123124
# Direct data publishing from CARLA for Autoware
124125
self.pub_pose_with_cov = self.ros2_node.create_publisher(
@@ -139,6 +140,9 @@ def setup(self):
139140
self.pub_gear_state = self.ros2_node.create_publisher(
140141
GearReport, "/vehicle/status/gear_status", 1
141142
)
143+
self.actuation_status = self.ros2_node.create_publisher(
144+
ActuationStatusStamped, "/vehicle/status/actuation_status", 1
145+
)
142146

143147
# Create Publisher for each Physical Sensors
144148
for sensor in self.sensors["sensors"]:
@@ -172,7 +176,6 @@ def __call__(self):
172176
input_data = self.sensor_interface.get_data()
173177
timestamp = GameTime.get_time()
174178
control = self.run_step(input_data, timestamp)
175-
control.manual_gear_shift = False
176179
return control
177180

178181
def get_param(self):
@@ -377,17 +380,10 @@ def imu(self, carla_imu_measurement):
377380

378381
def control_callback(self, in_cmd):
379382
"""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
391387
self.current_control = out_cmd
392388

393389
def ego_status(self):
@@ -398,6 +394,7 @@ def ego_status(self):
398394
ego_vehicle = CarlaDataProvider.get_actor_by_name(
399395
self.param_values["ego_vehicle_role_name"]
400396
)
397+
control = ego_vehicle.get_control()
401398

402399
self.current_vel = CarlaDataProvider.get_velocity(
403400
CarlaDataProvider.get_actor_by_name(self.param_values["ego_vehicle_role_name"])
@@ -408,6 +405,7 @@ def ego_status(self):
408405
out_ctrl_mode = ControlModeReport()
409406
out_gear_state = GearReport()
410407
out_traffic = TrafficSignalArray()
408+
out_actuation_status = ActuationStatusStamped()
411409

412410
out_vel_state.header = self.get_msg_header(frame_id="base_link")
413411
out_vel_state.longitudinal_velocity = self.current_vel
@@ -425,6 +423,14 @@ def ego_status(self):
425423
out_ctrl_mode.stamp = out_vel_state.header.stamp
426424
out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS
427425

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)
428434
self.pub_vel_state.publish(out_vel_state)
429435
self.pub_steering_state.publish(out_steering_state)
430436
self.pub_ctrl_mode.publish(out_ctrl_mode)
@@ -455,4 +461,4 @@ def run_step(self, input_data, timestamp):
455461

456462
# Publish ego vehicle status
457463
self.ego_status()
458-
return self.current_control
464+
return self.current_control

0 commit comments

Comments
 (0)