Skip to content

Commit

Permalink
fix steering and small cleanup of the interface
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem committed May 31, 2024
1 parent 9d16fe0 commit bf0a4b0
Show file tree
Hide file tree
Showing 4 changed files with 169 additions and 152 deletions.
2 changes: 1 addition & 1 deletion simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
csv_path_steer_map: $(find-pkg-share carla_autoware)/steer_map.csv
convert_accel_cmd: true
convert_brake_cmd: true
convert_steer_cmd: true
convert_steer_cmd: false
use_steer_ff: true
use_steer_fb: true
is_debugging: false
Expand Down
88 changes: 52 additions & 36 deletions simulator/carla_autoware/launch/carla_autoware.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,40 +1,56 @@
<launch>
<arg name="host" default="localhost"/>
<arg name="port" default="2000"/>
<arg name="timeout" default="20"/>
<arg name="ego_vehicle_role_name" default="ego_vehicle"/>
<arg name="vehicle_type" default="vehicle.toyota.prius"/>
<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.]"/>
<arg name="carla_map" default="Town01"/>
<arg name="sync_mode" default="True"/>
<arg name="fixed_delta_seconds" default="0.05" description="Timestep for CARLA, FPS=1/value"/>
<arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
<arg name="use_traffic_manager" default="False"/>
<arg name="max_real_delta_seconds" default="0.05"/>
<group>
<arg name="host" default="localhost"/>
<arg name="port" default="2000"/>
<arg name="timeout" default="20"/>
<arg name="ego_vehicle_role_name" default="ego_vehicle"/>
<arg name="vehicle_type" default="vehicle.toyota.prius"/>
<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.]"/>
<arg name="carla_map" default="Town01"/>
<arg name="sync_mode" default="True"/>
<arg name="fixed_delta_seconds" default="0.05" description="Timestep for CARLA, FPS=1/value"/>
<arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
<arg name="use_traffic_manager" default="False"/>
<arg name="max_real_delta_seconds" default="0.05"/>

<!-- CARLA Interface -->
<node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen">
<param name="host" value="$(var host)"/>
<param name="port" value="$(var port)"/>
<param name="timeout" value="$(var timeout)"/>
<param name="sync_mode" value="$(var sync_mode)"/>
<param name="fixed_delta_seconds" value="$(var fixed_delta_seconds)"/>
<param name="carla_map" value="$(var carla_map)"/>
<param name="ego_vehicle_role_name" value="$(var ego_vehicle_role_name)"/>
<param name="spawn_point" value="$(var spawn_point)"/>
<param name="vehicle_type" value="$(var vehicle_type)"/>
<param name="objects_definition_file" value="$(var objects_definition_file)"/>
<param name="use_traffic_manager" value="$(var use_traffic_manager)"/>
<param name="max_real_delta_seconds" value="$(var max_real_delta_seconds)"/>
</node>
<!-- CARLA Interface -->
<node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen">
<param name="host" value="$(var host)"/>
<param name="port" value="$(var port)"/>
<param name="timeout" value="$(var timeout)"/>
<param name="sync_mode" value="$(var sync_mode)"/>
<param name="fixed_delta_seconds" value="$(var fixed_delta_seconds)"/>
<param name="carla_map" value="$(var carla_map)"/>
<param name="ego_vehicle_role_name" value="$(var ego_vehicle_role_name)"/>
<param name="spawn_point" value="$(var spawn_point)"/>
<param name="vehicle_type" value="$(var vehicle_type)"/>
<param name="objects_definition_file" value="$(var objects_definition_file)"/>
<param name="use_traffic_manager" value="$(var use_traffic_manager)"/>
<param name="max_real_delta_seconds" value="$(var max_real_delta_seconds)"/>
</node>

<!-- Awsim configuration frame to CARLA frame -->
<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
pkg="tf2_ros"
exec="static_transform_publisher"
name="camera"
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 "
/>
<arg name="input_control_cmd" default="/control/command/control_cmd"/>
<arg name="input_odometry" default="/localization/kinematic_state"/>
<arg name="input_steering" default="/vehicle/status/steering_status"/>
<arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
<arg name="config_file" default="$(find-pkg-share carla_autoware)/raw_vehicle_cmd_converter.param.yaml"/>

<node pkg="raw_vehicle_cmd_converter" exec="raw_vehicle_cmd_converter_node" name="raw_vehicle_cmd_converter" output="screen">
<param from="$(var config_file)" allow_substs="true"/>
<remap from="~/input/control_cmd" to="$(var input_control_cmd)"/>
<remap from="~/input/odometry" to="$(var input_odometry)"/>
<remap from="~/input/steering" to="$(var input_steering)"/>
<remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
</node>

<!-- Awsim configuration frame to CARLA frame -->
<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
pkg="tf2_ros"
exec="static_transform_publisher"
name="camera"
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 "
/>
</group>
</launch>
156 changes: 75 additions & 81 deletions simulator/carla_autoware/src/carla_autoware/carla_autoware.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,14 @@


class SensorLoop(object):
def __init__(self, role_name):
def __init__(self):
self.start_game_time = None
self.start_system_time = None
self.sensor = None
self.ego_vehicle = None
self.ego_actor = None
self.running = False
self.timestamp_last_run = 0.0
self.timeout = 20.0
self.role_name = role_name

def _stop_loop(self):
self.running = False
Expand All @@ -48,7 +47,7 @@ def _tick_sensor(self, timestamp):
ego_action = self.sensor()
except SensorReceivedNoData as e:
raise RuntimeError(e)
self.ego_vehicle.apply_control(ego_action)
self.ego_actor.apply_control(ego_action)
if self.running:
CarlaDataProvider.get_world().tick()

Expand All @@ -59,7 +58,7 @@ def __init__(self):
self.param_ = self.interface.get_param()
self.world = None
self.sensor_wrapper = None
self.ego_vehicle = None
self.ego_actor = None
self.prev_tick_wall_time = 0.0

# Parameter for Initializing Carla World
Expand All @@ -80,83 +79,78 @@ def load_world(self):
client.set_timeout(self.timeout)
client.load_world(self.carla_map)
self.world = client.get_world()
if self.world is not None:
settings = self.world.get_settings()
settings.fixed_delta_seconds = self.fixed_delta_seconds
settings.synchronous_mode = self.sync_mode
self.world.apply_settings(settings)
CarlaDataProvider.set_world(self.world)
CarlaDataProvider.set_client(client)
spawn_point = carla.Transform()
spawn_point = carla.Transform()
point_items = self.spawn_point.split(",")
randomize = False
if len(point_items) == 6:
spawn_point.location.x = float(point_items[0])
spawn_point.location.y = float(point_items[1])
spawn_point.location.z = (
float(point_items[2]) + 2
) # +2 is used so the car did not stuck on the road when spawned.
spawn_point.rotation.roll = float(point_items[3])
spawn_point.rotation.pitch = float(point_items[4])
spawn_point.rotation.yaw = float(point_items[5])
else:
randomize = True
CarlaDataProvider.request_new_actor(
self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
)

self.sensor_wrapper = SensorWrapper(self.interface)
self.ego_vehicle = CarlaDataProvider.get_actor_by_name(self.agent_role_name)
self.sensor_wrapper.setup_sensors(self.ego_vehicle, False)
##########################################################################################################################################################
# TRAFFIC MANAGER
##########################################################################################################################################################
if self.use_traffic_manager:
traffic_manager = client.get_trafficmanager()
traffic_manager.set_synchronous_mode(True)
traffic_manager.set_random_device_seed(0)
random.seed(0)
spawn_points_tm = self.world.get_map().get_spawn_points()
for i, spawn_point in enumerate(spawn_points_tm):
self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
models = [
"dodge",
"audi",
"model3",
"mini",
"mustang",
"lincoln",
"prius",
"nissan",
"crown",
"impala",
]
blueprints = []
for vehicle in self.world.get_blueprint_library().filter("*vehicle*"):
if any(model in vehicle.id for model in models):
blueprints.append(vehicle)
max_vehicles = 30
max_vehicles = min([max_vehicles, len(spawn_points_tm)])
vehicles = []
for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
if temp is not None:
vehicles.append(temp)

for vehicle in vehicles:
vehicle.set_autopilot(True)

settings = self.world.get_settings()
settings.fixed_delta_seconds = self.fixed_delta_seconds
settings.synchronous_mode = self.sync_mode
self.world.apply_settings(settings)
CarlaDataProvider.set_world(self.world)
CarlaDataProvider.set_client(client)
spawn_point = carla.Transform()
point_items = self.spawn_point.split(",")
randomize = False
if len(point_items) == 6:
spawn_point.location.x = float(point_items[0])
spawn_point.location.y = float(point_items[1])
spawn_point.location.z = (
float(point_items[2]) + 2
) # +2 is used so the car did not stuck on the road when spawned.
spawn_point.rotation.roll = float(point_items[3])
spawn_point.rotation.pitch = float(point_items[4])
spawn_point.rotation.yaw = float(point_items[5])
else:
print("Carla Interface Couldn't find the world, Carla is not Running")
randomize = True
self.ego_actor = CarlaDataProvider.request_new_actor(
self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
)
self.interface.ego_actor = self.ego_actor # TODO improve design
self.interface.physics_control = self.ego_actor.get_physics_control()

self.sensor_wrapper = SensorWrapper(self.interface)
self.sensor_wrapper.setup_sensors(self.ego_actor, False)
##########################################################################################################################################################
# TRAFFIC MANAGER
##########################################################################################################################################################
if self.use_traffic_manager:
traffic_manager = client.get_trafficmanager()
traffic_manager.set_synchronous_mode(True)
traffic_manager.set_random_device_seed(0)
random.seed(0)
spawn_points_tm = self.world.get_map().get_spawn_points()
for i, spawn_point in enumerate(spawn_points_tm):
self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
models = [
"dodge",
"audi",
"model3",
"mini",
"mustang",
"lincoln",
"prius",
"nissan",
"crown",
"impala",
]
blueprints = []
for vehicle in self.world.get_blueprint_library().filter("*vehicle*"):
if any(model in vehicle.id for model in models):
blueprints.append(vehicle)
max_vehicles = 30
max_vehicles = min([max_vehicles, len(spawn_points_tm)])
vehicles = []
for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
if temp is not None:
vehicles.append(temp)

for vehicle in vehicles:
vehicle.set_autopilot(True)

def run_bridge(self):
self.bridge_loop = SensorLoop(self.agent_role_name)
self.bridge_loop = SensorLoop()
self.bridge_loop.sensor = self.sensor_wrapper
self.bridge_loop.ego_vehicle = self.ego_vehicle
self.bridge_loop.ego_actor = self.ego_actor
self.bridge_loop.start_system_time = time.time()
self.bridge_loop.start_game_time = GameTime.get_time()
self.bridge_loop.role_name = self.agent_role_name
self.bridge_loop.running = True
while self.bridge_loop.running:
timestamp = None
Expand All @@ -179,20 +173,20 @@ def _stop_loop(self, signum, frame):
def _cleanup(self):
self.sensor_wrapper.cleanup()
CarlaDataProvider.cleanup()
if self.ego_vehicle:
self.ego_vehicle.destroy()
self.ego_vehicle = None
if self.ego_actor:
self.ego_actor.destroy()
self.ego_actor = None

if self.interface:
self.interface.shutdown()
self.interface = None


def main():
carla_bridge = InitializeInterface()
carla_bridge.load_world()
stop_bridge = signal.signal(signal.SIGINT, carla_bridge._stop_loop)
signal.signal(signal.SIGINT, carla_bridge._stop_loop)
carla_bridge.run_bridge()
signal.signal(signal.SIGINT, stop_bridge)
carla_bridge._cleanup()


Expand Down
Loading

0 comments on commit bf0a4b0

Please sign in to comment.