Skip to content

Commit bf0a4b0

Browse files
maxime-clemmaxime-clem
committed
fix steering and small cleanup of the interface
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 9d16fe0 commit bf0a4b0

File tree

4 files changed

+169
-152
lines changed

4 files changed

+169
-152
lines changed

‎simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
csv_path_steer_map: $(find-pkg-share carla_autoware)/steer_map.csv
66
convert_accel_cmd: true
77
convert_brake_cmd: true
8-
convert_steer_cmd: true
8+
convert_steer_cmd: false
99
use_steer_ff: true
1010
use_steer_fb: true
1111
is_debugging: false

‎simulator/carla_autoware/launch/carla_autoware.launch.xml

+52-36
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,56 @@
11
<launch>
2-
<arg name="host" default="localhost"/>
3-
<arg name="port" default="2000"/>
4-
<arg name="timeout" default="20"/>
5-
<arg name="ego_vehicle_role_name" default="ego_vehicle"/>
6-
<arg name="vehicle_type" default="vehicle.toyota.prius"/>
7-
<arg name="spawn_point" default="None" description="location of ego vehicle spawn (default is random), example = [0.2 , 4.1, 0.4, 0., 0., 0.]"/>
8-
<arg name="carla_map" default="Town01"/>
9-
<arg name="sync_mode" default="True"/>
10-
<arg name="fixed_delta_seconds" default="0.05" description="Timestep for CARLA, FPS=1/value"/>
11-
<arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
12-
<arg name="use_traffic_manager" default="False"/>
13-
<arg name="max_real_delta_seconds" default="0.05"/>
2+
<group>
3+
<arg name="host" default="localhost"/>
4+
<arg name="port" default="2000"/>
5+
<arg name="timeout" default="20"/>
6+
<arg name="ego_vehicle_role_name" default="ego_vehicle"/>
7+
<arg name="vehicle_type" default="vehicle.toyota.prius"/>
8+
<arg name="spawn_point" default="None" description="location of ego vehicle spawn (default is random), example = [0.2 , 4.1, 0.4, 0., 0., 0.]"/>
9+
<arg name="carla_map" default="Town01"/>
10+
<arg name="sync_mode" default="True"/>
11+
<arg name="fixed_delta_seconds" default="0.05" description="Timestep for CARLA, FPS=1/value"/>
12+
<arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
13+
<arg name="use_traffic_manager" default="False"/>
14+
<arg name="max_real_delta_seconds" default="0.05"/>
1415

15-
<!-- CARLA Interface -->
16-
<node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen">
17-
<param name="host" value="$(var host)"/>
18-
<param name="port" value="$(var port)"/>
19-
<param name="timeout" value="$(var timeout)"/>
20-
<param name="sync_mode" value="$(var sync_mode)"/>
21-
<param name="fixed_delta_seconds" value="$(var fixed_delta_seconds)"/>
22-
<param name="carla_map" value="$(var carla_map)"/>
23-
<param name="ego_vehicle_role_name" value="$(var ego_vehicle_role_name)"/>
24-
<param name="spawn_point" value="$(var spawn_point)"/>
25-
<param name="vehicle_type" value="$(var vehicle_type)"/>
26-
<param name="objects_definition_file" value="$(var objects_definition_file)"/>
27-
<param name="use_traffic_manager" value="$(var use_traffic_manager)"/>
28-
<param name="max_real_delta_seconds" value="$(var max_real_delta_seconds)"/>
29-
</node>
16+
<!-- CARLA Interface -->
17+
<node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen">
18+
<param name="host" value="$(var host)"/>
19+
<param name="port" value="$(var port)"/>
20+
<param name="timeout" value="$(var timeout)"/>
21+
<param name="sync_mode" value="$(var sync_mode)"/>
22+
<param name="fixed_delta_seconds" value="$(var fixed_delta_seconds)"/>
23+
<param name="carla_map" value="$(var carla_map)"/>
24+
<param name="ego_vehicle_role_name" value="$(var ego_vehicle_role_name)"/>
25+
<param name="spawn_point" value="$(var spawn_point)"/>
26+
<param name="vehicle_type" value="$(var vehicle_type)"/>
27+
<param name="objects_definition_file" value="$(var objects_definition_file)"/>
28+
<param name="use_traffic_manager" value="$(var use_traffic_manager)"/>
29+
<param name="max_real_delta_seconds" value="$(var max_real_delta_seconds)"/>
30+
</node>
3031

31-
<!-- Awsim configuration frame to CARLA frame -->
32-
<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 "/>
33-
<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 "/>
34-
<node
35-
pkg="tf2_ros"
36-
exec="static_transform_publisher"
37-
name="camera"
38-
args="-0.05 -0.0175 1.1 0.0364 -0.015 0.001 /traffic_light_left_camera/camera_link /traffic_light_left_camera/camera_link_changed "
39-
/>
32+
<arg name="input_control_cmd" default="/control/command/control_cmd"/>
33+
<arg name="input_odometry" default="/localization/kinematic_state"/>
34+
<arg name="input_steering" default="/vehicle/status/steering_status"/>
35+
<arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
36+
<arg name="config_file" default="$(find-pkg-share carla_autoware)/raw_vehicle_cmd_converter.param.yaml"/>
37+
38+
<node pkg="raw_vehicle_cmd_converter" exec="raw_vehicle_cmd_converter_node" name="raw_vehicle_cmd_converter" output="screen">
39+
<param from="$(var config_file)" allow_substs="true"/>
40+
<remap from="~/input/control_cmd" to="$(var input_control_cmd)"/>
41+
<remap from="~/input/odometry" to="$(var input_odometry)"/>
42+
<remap from="~/input/steering" to="$(var input_steering)"/>
43+
<remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
44+
</node>
45+
46+
<!-- Awsim configuration frame to CARLA frame -->
47+
<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 "/>
48+
<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 "/>
49+
<node
50+
pkg="tf2_ros"
51+
exec="static_transform_publisher"
52+
name="camera"
53+
args="-0.05 -0.0175 1.1 0.0364 -0.015 0.001 /traffic_light_left_camera/camera_link /traffic_light_left_camera/camera_link_changed "
54+
/>
55+
</group>
4056
</launch>

‎simulator/carla_autoware/src/carla_autoware/carla_autoware.py

+75-81
Original file line numberDiff line numberDiff line change
@@ -26,15 +26,14 @@
2626

2727

2828
class SensorLoop(object):
29-
def __init__(self, role_name):
29+
def __init__(self):
3030
self.start_game_time = None
3131
self.start_system_time = None
3232
self.sensor = None
33-
self.ego_vehicle = None
33+
self.ego_actor = None
3434
self.running = False
3535
self.timestamp_last_run = 0.0
3636
self.timeout = 20.0
37-
self.role_name = role_name
3837

3938
def _stop_loop(self):
4039
self.running = False
@@ -48,7 +47,7 @@ def _tick_sensor(self, timestamp):
4847
ego_action = self.sensor()
4948
except SensorReceivedNoData as e:
5049
raise RuntimeError(e)
51-
self.ego_vehicle.apply_control(ego_action)
50+
self.ego_actor.apply_control(ego_action)
5251
if self.running:
5352
CarlaDataProvider.get_world().tick()
5453

@@ -59,7 +58,7 @@ def __init__(self):
5958
self.param_ = self.interface.get_param()
6059
self.world = None
6160
self.sensor_wrapper = None
62-
self.ego_vehicle = None
61+
self.ego_actor = None
6362
self.prev_tick_wall_time = 0.0
6463

6564
# Parameter for Initializing Carla World
@@ -80,83 +79,78 @@ def load_world(self):
8079
client.set_timeout(self.timeout)
8180
client.load_world(self.carla_map)
8281
self.world = client.get_world()
83-
if self.world is not None:
84-
settings = self.world.get_settings()
85-
settings.fixed_delta_seconds = self.fixed_delta_seconds
86-
settings.synchronous_mode = self.sync_mode
87-
self.world.apply_settings(settings)
88-
CarlaDataProvider.set_world(self.world)
89-
CarlaDataProvider.set_client(client)
90-
spawn_point = carla.Transform()
91-
spawn_point = carla.Transform()
92-
point_items = self.spawn_point.split(",")
93-
randomize = False
94-
if len(point_items) == 6:
95-
spawn_point.location.x = float(point_items[0])
96-
spawn_point.location.y = float(point_items[1])
97-
spawn_point.location.z = (
98-
float(point_items[2]) + 2
99-
) # +2 is used so the car did not stuck on the road when spawned.
100-
spawn_point.rotation.roll = float(point_items[3])
101-
spawn_point.rotation.pitch = float(point_items[4])
102-
spawn_point.rotation.yaw = float(point_items[5])
103-
else:
104-
randomize = True
105-
CarlaDataProvider.request_new_actor(
106-
self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
107-
)
108-
109-
self.sensor_wrapper = SensorWrapper(self.interface)
110-
self.ego_vehicle = CarlaDataProvider.get_actor_by_name(self.agent_role_name)
111-
self.sensor_wrapper.setup_sensors(self.ego_vehicle, False)
112-
##########################################################################################################################################################
113-
# TRAFFIC MANAGER
114-
##########################################################################################################################################################
115-
if self.use_traffic_manager:
116-
traffic_manager = client.get_trafficmanager()
117-
traffic_manager.set_synchronous_mode(True)
118-
traffic_manager.set_random_device_seed(0)
119-
random.seed(0)
120-
spawn_points_tm = self.world.get_map().get_spawn_points()
121-
for i, spawn_point in enumerate(spawn_points_tm):
122-
self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
123-
models = [
124-
"dodge",
125-
"audi",
126-
"model3",
127-
"mini",
128-
"mustang",
129-
"lincoln",
130-
"prius",
131-
"nissan",
132-
"crown",
133-
"impala",
134-
]
135-
blueprints = []
136-
for vehicle in self.world.get_blueprint_library().filter("*vehicle*"):
137-
if any(model in vehicle.id for model in models):
138-
blueprints.append(vehicle)
139-
max_vehicles = 30
140-
max_vehicles = min([max_vehicles, len(spawn_points_tm)])
141-
vehicles = []
142-
for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
143-
temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
144-
if temp is not None:
145-
vehicles.append(temp)
146-
147-
for vehicle in vehicles:
148-
vehicle.set_autopilot(True)
149-
82+
settings = self.world.get_settings()
83+
settings.fixed_delta_seconds = self.fixed_delta_seconds
84+
settings.synchronous_mode = self.sync_mode
85+
self.world.apply_settings(settings)
86+
CarlaDataProvider.set_world(self.world)
87+
CarlaDataProvider.set_client(client)
88+
spawn_point = carla.Transform()
89+
point_items = self.spawn_point.split(",")
90+
randomize = False
91+
if len(point_items) == 6:
92+
spawn_point.location.x = float(point_items[0])
93+
spawn_point.location.y = float(point_items[1])
94+
spawn_point.location.z = (
95+
float(point_items[2]) + 2
96+
) # +2 is used so the car did not stuck on the road when spawned.
97+
spawn_point.rotation.roll = float(point_items[3])
98+
spawn_point.rotation.pitch = float(point_items[4])
99+
spawn_point.rotation.yaw = float(point_items[5])
150100
else:
151-
print("Carla Interface Couldn't find the world, Carla is not Running")
101+
randomize = True
102+
self.ego_actor = CarlaDataProvider.request_new_actor(
103+
self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
104+
)
105+
self.interface.ego_actor = self.ego_actor # TODO improve design
106+
self.interface.physics_control = self.ego_actor.get_physics_control()
107+
108+
self.sensor_wrapper = SensorWrapper(self.interface)
109+
self.sensor_wrapper.setup_sensors(self.ego_actor, False)
110+
##########################################################################################################################################################
111+
# TRAFFIC MANAGER
112+
##########################################################################################################################################################
113+
if self.use_traffic_manager:
114+
traffic_manager = client.get_trafficmanager()
115+
traffic_manager.set_synchronous_mode(True)
116+
traffic_manager.set_random_device_seed(0)
117+
random.seed(0)
118+
spawn_points_tm = self.world.get_map().get_spawn_points()
119+
for i, spawn_point in enumerate(spawn_points_tm):
120+
self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
121+
models = [
122+
"dodge",
123+
"audi",
124+
"model3",
125+
"mini",
126+
"mustang",
127+
"lincoln",
128+
"prius",
129+
"nissan",
130+
"crown",
131+
"impala",
132+
]
133+
blueprints = []
134+
for vehicle in self.world.get_blueprint_library().filter("*vehicle*"):
135+
if any(model in vehicle.id for model in models):
136+
blueprints.append(vehicle)
137+
max_vehicles = 30
138+
max_vehicles = min([max_vehicles, len(spawn_points_tm)])
139+
vehicles = []
140+
for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
141+
temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
142+
if temp is not None:
143+
vehicles.append(temp)
144+
145+
for vehicle in vehicles:
146+
vehicle.set_autopilot(True)
152147

153148
def run_bridge(self):
154-
self.bridge_loop = SensorLoop(self.agent_role_name)
149+
self.bridge_loop = SensorLoop()
155150
self.bridge_loop.sensor = self.sensor_wrapper
156-
self.bridge_loop.ego_vehicle = self.ego_vehicle
151+
self.bridge_loop.ego_actor = self.ego_actor
157152
self.bridge_loop.start_system_time = time.time()
158153
self.bridge_loop.start_game_time = GameTime.get_time()
159-
self.bridge_loop.role_name = self.agent_role_name
160154
self.bridge_loop.running = True
161155
while self.bridge_loop.running:
162156
timestamp = None
@@ -179,20 +173,20 @@ def _stop_loop(self, signum, frame):
179173
def _cleanup(self):
180174
self.sensor_wrapper.cleanup()
181175
CarlaDataProvider.cleanup()
182-
if self.ego_vehicle:
183-
self.ego_vehicle.destroy()
184-
self.ego_vehicle = None
176+
if self.ego_actor:
177+
self.ego_actor.destroy()
178+
self.ego_actor = None
185179

186180
if self.interface:
181+
self.interface.shutdown()
187182
self.interface = None
188183

189184

190185
def main():
191186
carla_bridge = InitializeInterface()
192187
carla_bridge.load_world()
193-
stop_bridge = signal.signal(signal.SIGINT, carla_bridge._stop_loop)
188+
signal.signal(signal.SIGINT, carla_bridge._stop_loop)
194189
carla_bridge.run_bridge()
195-
signal.signal(signal.SIGINT, stop_bridge)
196190
carla_bridge._cleanup()
197191

198192

0 commit comments

Comments
 (0)