class CarlaRosBridge(object):
"""
Carla Ros bridge
"""
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
rospy.init_node("carla_bridge", anonymous=True)
self.parameters = params
self.actors = {}
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
# set carla world settings
self.carla_settings = carla_world.get_settings()
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode)
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
# register callback to create/delete actors
self.update_child_actors_lock = Lock()
self.carla_world.on_tick(self._carla_update_child_actors)
# register callback to update actors
self.carla_world.on_tick(self._carla_time_tick)
self.pseudo_actors = []
# add map
self.pseudo_actors.append(
Map(carla_world=self.carla_world, communication=self.comm))
# add global object sensor
self.pseudo_actors.append(
ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
def destroy(self):
"""
Function to destroy this object.
:return:
"""
rospy.signal_shutdown("")
self.carla_control_queue.put(CarlaControl.STEP_ONCE)
if not self.carla_settings.synchronous_mode:
self.update_child_actors_lock.acquire()
self.update_lock.acquire()
rospy.loginfo("Exiting Bridge")
def process_run_state(self):
"""
process state changes
"""
command = None
# get last command
while not self.carla_control_queue.empty():
command = self.carla_control_queue.get()
while not command is None and not rospy.is_shutdown():
self.carla_run_state = command
if self.carla_run_state == CarlaControl.PAUSE:
# wait for next command
rospy.loginfo("State set to PAUSED")
self.status_publisher.set_synchronous_mode_running(False)
command = self.carla_control_queue.get()
elif self.carla_run_state == CarlaControl.PLAY:
rospy.loginfo("State set to PLAY")
self.status_publisher.set_synchronous_mode_running(True)
return
elif self.carla_run_state == CarlaControl.STEP_ONCE:
rospy.loginfo("Execute single step.")
self.status_publisher.set_synchronous_mode_running(True)
self.carla_control_queue.put(CarlaControl.PAUSE)
return
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
while not rospy.is_shutdown():
self.process_run_state()
self._update_actors()
frame = self.carla_world.tick()
world_snapshot = self.carla_world.get_snapshot()
self.status_publisher.set_frame(frame)
self.comm.update_clock(world_snapshot.timestamp)
rospy.logdebug(
"Tick for frame {} returned. Waiting for sensor data...".
format(frame))
self._update(frame, world_snapshot.timestamp.elapsed_seconds)
rospy.logdebug("Waiting for sensor data finished.")
self.comm.send_msgs()
def _carla_time_tick(self, carla_timestamp):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates.
After successful locking the update mutex
(only perform trylock to respect bridge processing time)
the clock and the children are updated.
Finally the ROS messages collected to be published are sent out.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if not rospy.is_shutdown():
if self.update_lock.acquire(False):
if self.timestamp_last_run < carla_timestamp.elapsed_seconds:
self.timestamp_last_run = carla_timestamp.elapsed_seconds
self.comm.update_clock(carla_timestamp)
self._update(carla_timestamp.frame,
carla_timestamp.elapsed_seconds)
self.comm.send_msgs()
self.update_lock.release()
def _carla_update_child_actors(self, _):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates of the actors
After successful locking the mutex
(only perform trylock to respect bridge processing time)
the existing actors are updated.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if not rospy.is_shutdown():
if self.update_child_actors_lock.acquire(False):
self._update_actors()
# actors are only created/deleted around once per second
time.sleep(1)
self.update_child_actors_lock.release()
def _update_actors(self):
"""
update the available actors
"""
carla_actors = self.carla_world.get_actors()
actors_updated = False
# Add new actors
for actor in carla_actors:
if actor.id not in self.actors:
if self._create_actor(actor):
actors_updated = True
# create list of carla actors ids
carla_actor_ids = []
for actor in carla_actors:
carla_actor_ids.append(actor.id)
# remove non-existing actors
ids_to_delete = []
for actor_id in self.actors:
if actor_id not in carla_actor_ids:
ids_to_delete.append(actor_id)
actors_updated = True
if ids_to_delete:
with self.update_lock:
for id_to_delete in ids_to_delete:
# remove actor
actor = self.actors[id_to_delete]
rospy.loginfo(
"Remove {}(id={}, parent_id={}, prefix={})".format(
actor.__class__.__name__, actor.get_id(),
actor.get_parent_id(), actor.get_prefix()))
actor.destroy()
del self.actors[id_to_delete]
# remove pseudo-actors that have actor as parent
updated_pseudo_actors = []
for pseudo_actor in self.pseudo_actors:
if pseudo_actor.get_parent_id() == id_to_delete:
rospy.loginfo(
"Remove {}(parent_id={}, prefix={})".format(
pseudo_actor.__class__.__name__,
pseudo_actor.get_parent_id(),
pseudo_actor.get_prefix()))
pseudo_actor.destroy()
del pseudo_actor
else:
updated_pseudo_actors.append(pseudo_actor)
self.pseudo_actors = updated_pseudo_actors
if actors_updated:
self.publish_actor_list()
def publish_actor_list(self):
"""
publish list of carla actors
:return:
"""
ros_actor_list = CarlaActorList()
with self.update_lock:
for actor_id in self.actors:
actor = self.actors[actor_id].carla_actor
ros_actor = CarlaActorInfo()
ros_actor.id = actor.id
ros_actor.type = actor.type_id
try:
ros_actor.rolename = str(actor.attributes.get('role_name'))
except ValueError:
pass
if actor.parent:
ros_actor.parent_id = actor.parent.id
else:
ros_actor.parent_id = 0
ros_actor_list.actors.append(ros_actor)
self.comm.publish_message("/carla/actor_list",
ros_actor_list,
is_latched=True)
def _create_actor(self, carla_actor): # pylint: disable=too-many-branches,too-many-statements
"""
create an actor
"""
parent = None
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
parent = self._create_actor(carla_actor.parent)
actor = None
pseudo_actors = []
if carla_actor.type_id.startswith('traffic'):
if carla_actor.type_id == "traffic.traffic_light":
actor = TrafficLight(carla_actor, parent, self.comm)
else:
actor = Traffic(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("vehicle"):
if carla_actor.attributes.get('role_name')\
in self.parameters['ego_vehicle']['role_name']:
ego_vehicle = EgoVehicle(carla_actor, parent, self.comm)
actor = ego_vehicle
pseudo_actors.append(
ObjectSensor(parent=ego_vehicle,
communication=self.comm,
actor_list=self.actors,
filtered_id=carla_actor.id))
else:
actor = Vehicle(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("sensor"):
if carla_actor.type_id.startswith("sensor.camera"):
if carla_actor.type_id.startswith("sensor.camera.rgb"):
actor = RgbCamera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.depth"):
actor = DepthCamera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith(
"sensor.camera.semantic_segmentation"):
actor = SemanticSegmentationCamera(
carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
else:
actor = Camera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.lidar"):
actor = Lidar(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.gnss"):
actor = Gnss(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.collision"):
actor = CollisionSensor(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.lane_invasion"):
actor = LaneInvasionSensor(
carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
else:
actor = Sensor(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("spectator"):
actor = Spectator(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("walker"):
actor = Walker(carla_actor, parent, self.comm)
else:
actor = Actor(carla_actor, parent, self.comm)
rospy.loginfo("Created {}(id={}, parent_id={},"
" type={}, prefix={}, attributes={})".format(
actor.__class__.__name__, actor.get_id(),
actor.get_parent_id(), carla_actor.type_id,
actor.get_prefix(), carla_actor.attributes))
with self.update_lock:
self.actors[carla_actor.id] = actor
for pseudo_actor in pseudo_actors:
rospy.loginfo("Created {}(parent_id={}, prefix={})".format(
pseudo_actor.__class__.__name__, pseudo_actor.get_parent_id(),
pseudo_actor.get_prefix()))
with self.update_lock:
self.pseudo_actors.append(pseudo_actor)
return actor
def run(self):
"""
Run the bridge functionality.
Registers on shutdown callback at rospy and spins ROS.
:return:
"""
rospy.on_shutdown(self.on_shutdown)
rospy.spin()
def on_shutdown(self):
"""
Function to be called on shutdown.
This function is registered at rospy as shutdown handler.
"""
rospy.loginfo("Shutdown requested")
self.destroy()
def _update(self, frame_id, timestamp):
"""
update all actors
:return:
"""
# update all pseudo actors
for actor in self.pseudo_actors:
actor.update(frame_id, timestamp)
# update all carla actors
for actor_id in self.actors:
try:
self.actors[actor_id].update(frame_id, timestamp)
except RuntimeError as e:
rospy.logwarn("Update actor {}({}) failed: {}".format(
self.actors[actor_id].__class__.__name__, actor_id, e))
continue
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
rospy.init_node("carla_bridge", anonymous=True)
self.parameters = params
self.actors = {}
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
# set carla world settings
self.carla_settings = carla_world.get_settings()
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode)
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
# register callback to create/delete actors
self.update_child_actors_lock = Lock()
self.carla_world.on_tick(self._carla_update_child_actors)
# register callback to update actors
self.carla_world.on_tick(self._carla_time_tick)
self.pseudo_actors = []
# add map
self.pseudo_actors.append(
Map(carla_world=self.carla_world, communication=self.comm))
# add global object sensor
self.pseudo_actors.append(
ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
class CarlaRosBridge(object):
"""
Carla Ros bridge
"""
CARLA_VERSION = "0.9.6"
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
# check CARLA version
dist = pkg_resources.get_distribution("carla")
if LooseVersion(dist.version) < LooseVersion(self.CARLA_VERSION):
raise ImportError(
"CARLA version {} or newer required. CARLA version found: {}".
format(self.CARLA_VERSION, dist))
self.parameters = params
self.actors = {}
self.pseudo_actors = []
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
# set carla world settings
self.carla_settings = carla_world.get_settings()
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.update_actors_queue = queue.Queue(maxsize=1)
# start thread to update actors
self.update_actor_thread = Thread(
target=self._update_actors_thread)
self.update_actor_thread.start()
# create initially existing actors
self.update_actors_queue.put(
set([x.id for x in self.carla_world.get_snapshot()]))
# wait for first actors creation to be finished
self.update_actors_queue.join()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# add world info
self.pseudo_actors.append(
WorldInfo(carla_world=self.carla_world, communication=self.comm))
# add global object sensor
self.pseudo_actors.append(
ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
self.debug_helper = DebugHelper(carla_world.debug)
# add traffic light pseudo sensor
self.pseudo_actors.append(
TrafficLightsSensor(parent=None,
communication=self.comm,
actor_list=self.actors))
def destroy(self):
"""
Function to destroy this object.
:return:
"""
rospy.signal_shutdown("")
self.debug_helper.destroy()
self.shutdown.set()
self.carla_control_queue.put(CarlaControl.STEP_ONCE)
if not self.carla_settings.synchronous_mode:
if self.on_tick_id:
self.carla_world.remove_on_tick(self.on_tick_id)
self.update_actor_thread.join()
self._update_actors(set())
rospy.loginfo("Exiting Bridge")
def process_run_state(self):
"""
process state changes
"""
command = None
# get last command
while not self.carla_control_queue.empty():
command = self.carla_control_queue.get()
while command is not None and not rospy.is_shutdown():
self.carla_run_state = command
if self.carla_run_state == CarlaControl.PAUSE:
# wait for next command
rospy.loginfo("State set to PAUSED")
self.status_publisher.set_synchronous_mode_running(False)
command = self.carla_control_queue.get()
elif self.carla_run_state == CarlaControl.PLAY:
rospy.loginfo("State set to PLAY")
self.status_publisher.set_synchronous_mode_running(True)
return
elif self.carla_run_state == CarlaControl.STEP_ONCE:
rospy.loginfo("Execute single step.")
self.status_publisher.set_synchronous_mode_running(True)
self.carla_control_queue.put(CarlaControl.PAUSE)
return
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
while not self.shutdown.is_set():
self.process_run_state()
if self.parameters[
'synchronous_mode_wait_for_vehicle_control_command']:
# fill list of available ego vehicles
self._expected_ego_vehicle_control_command_ids = []
with self._expected_ego_vehicle_control_command_ids_lock:
for actor_id, actor in self.actors.iteritems():
if isinstance(actor, EgoVehicle):
self._expected_ego_vehicle_control_command_ids.append(
actor_id)
frame = self.carla_world.tick()
world_snapshot = self.carla_world.get_snapshot()
self.status_publisher.set_frame(frame)
self.comm.update_clock(world_snapshot.timestamp)
rospy.logdebug(
"Tick for frame {} returned. Waiting for sensor data...".
format(frame))
self._update(frame, world_snapshot.timestamp.elapsed_seconds)
rospy.logdebug("Waiting for sensor data finished.")
self.comm.send_msgs()
self._update_actors(set([x.id for x in world_snapshot]))
if self.parameters[
'synchronous_mode_wait_for_vehicle_control_command']:
# wait for all ego vehicles to send a vehicle control command
if self._expected_ego_vehicle_control_command_ids:
if not self._all_vehicle_control_commands_received.wait(1):
rospy.logwarn(
"Timeout (1s) while waiting for vehicle control commands. "
"Missing command from actor ids {}".format(
self._expected_ego_vehicle_control_command_ids)
)
self._all_vehicle_control_commands_received.clear()
def _carla_time_tick(self, carla_snapshot):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates.
After successful locking the update mutex
(only perform trylock to respect bridge processing time)
the clock and the children are updated.
Finally the ROS messages collected to be published are sent out.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if not self.shutdown.is_set():
if self.update_lock.acquire(False):
if self.timestamp_last_run < carla_snapshot.timestamp.elapsed_seconds:
self.timestamp_last_run = carla_snapshot.timestamp.elapsed_seconds
self.comm.update_clock(carla_snapshot.timestamp)
self.status_publisher.set_frame(carla_snapshot.frame)
self._update(carla_snapshot.frame,
carla_snapshot.timestamp.elapsed_seconds)
self.comm.send_msgs()
self.update_lock.release()
# if possible push current snapshot to update-actors-thread
try:
self.update_actors_queue.put_nowait(
set([x.id for x in carla_snapshot]))
except queue.Full:
pass
def _update_actors_thread(self):
"""
execution loop for async mode actor list updates
"""
while not self.shutdown.is_set():
try:
current_actors = self.update_actors_queue.get(timeout=1)
if current_actors:
self._update_actors(current_actors)
self.update_actors_queue.task_done()
except queue.Empty:
pass
def _update_actors(self, current_actors):
"""
update the available actors
"""
previous_actors = set(self.actors)
new_actors = current_actors - previous_actors
deleted_actors = previous_actors - current_actors
if new_actors:
for carla_actor in self.carla_world.get_actors(list(new_actors)):
self._create_actor(carla_actor)
if deleted_actors:
for id_to_delete in deleted_actors:
# remove actor
actor = self.actors[id_to_delete]
with self.update_lock:
rospy.loginfo(
"Remove {}(id={}, parent_id={}, prefix={})".format(
actor.__class__.__name__, actor.get_id(),
actor.get_parent_id(), actor.get_prefix()))
actor.destroy()
del self.actors[id_to_delete]
# remove pseudo-actors that have actor as parent
updated_pseudo_actors = []
for pseudo_actor in self.pseudo_actors:
if pseudo_actor.get_parent_id() == id_to_delete:
rospy.loginfo(
"Remove {}(parent_id={}, prefix={})".format(
pseudo_actor.__class__.__name__,
pseudo_actor.get_parent_id(),
pseudo_actor.get_prefix()))
pseudo_actor.destroy()
del pseudo_actor
else:
updated_pseudo_actors.append(pseudo_actor)
self.pseudo_actors = updated_pseudo_actors
# publish actor list on change
if new_actors or deleted_actors:
self.publish_actor_list()
def publish_actor_list(self):
"""
publish list of carla actors
:return:
"""
ros_actor_list = CarlaActorList()
for actor_id in self.actors:
actor = self.actors[actor_id].carla_actor
ros_actor = CarlaActorInfo()
ros_actor.id = actor.id
ros_actor.type = actor.type_id
try:
ros_actor.rolename = str(actor.attributes.get('role_name'))
except ValueError:
pass
if actor.parent:
ros_actor.parent_id = actor.parent.id
else:
ros_actor.parent_id = 0
ros_actor_list.actors.append(ros_actor)
self.comm.publish_message("/carla/actor_list",
ros_actor_list,
is_latched=True)
def _create_actor(self, carla_actor): # pylint: disable=too-many-branches,too-many-statements
"""
create an actor
"""
parent = None
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
parent = self._create_actor(carla_actor.parent)
actor = None
pseudo_actors = []
if carla_actor.type_id.startswith('traffic'):
if carla_actor.type_id == "traffic.traffic_light":
actor = TrafficLight(carla_actor, parent, self.comm)
else:
actor = Traffic(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("vehicle"):
if carla_actor.attributes.get('role_name')\
in self.parameters['ego_vehicle']['role_name']:
actor = EgoVehicle(carla_actor, parent, self.comm,
self._ego_vehicle_control_applied_callback)
pseudo_actors.append(
ObjectSensor(parent=actor,
communication=self.comm,
actor_list=self.actors,
filtered_id=carla_actor.id))
else:
actor = Vehicle(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("sensor"):
if carla_actor.type_id.startswith("sensor.camera"):
if carla_actor.type_id.startswith("sensor.camera.rgb"):
actor = RgbCamera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.depth"):
actor = DepthCamera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith(
"sensor.camera.semantic_segmentation"):
actor = SemanticSegmentationCamera(
carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
else:
actor = Camera(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.lidar"):
actor = Lidar(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.radar"):
actor = Radar(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.gnss"):
actor = Gnss(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.imu"):
actor = ImuSensor(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.collision"):
actor = CollisionSensor(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.lane_invasion"):
actor = LaneInvasionSensor(
carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
else:
actor = Sensor(carla_actor, parent, self.comm,
self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("spectator"):
actor = Spectator(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("walker"):
actor = Walker(carla_actor, parent, self.comm)
else:
actor = Actor(carla_actor, parent, self.comm)
rospy.loginfo("Created {}(id={}, parent_id={},"
" type={}, prefix={}, attributes={})".format(
actor.__class__.__name__, actor.get_id(),
actor.get_parent_id(), carla_actor.type_id,
actor.get_prefix(), carla_actor.attributes))
with self.update_lock:
self.actors[carla_actor.id] = actor
for pseudo_actor in pseudo_actors:
rospy.loginfo("Created {}(parent_id={}, prefix={})".format(
pseudo_actor.__class__.__name__, pseudo_actor.get_parent_id(),
pseudo_actor.get_prefix()))
with self.update_lock:
self.pseudo_actors.append(pseudo_actor)
return actor
def run(self):
"""
Run the bridge functionality.
Registers on shutdown callback at rospy and spins ROS.
:return:
"""
rospy.on_shutdown(self.on_shutdown)
rospy.spin()
def on_shutdown(self):
"""
Function to be called on shutdown.
This function is registered at rospy as shutdown handler.
"""
rospy.loginfo("Shutdown requested")
self.destroy()
def _update(self, frame_id, timestamp):
"""
update all actors
:return:
"""
# update all pseudo actors
for actor in self.pseudo_actors:
actor.update(frame_id, timestamp)
# update all carla actors
for actor_id in self.actors:
try:
self.actors[actor_id].update(frame_id, timestamp)
except RuntimeError as e:
rospy.logwarn("Update actor {}({}) failed: {}".format(
self.actors[actor_id].__class__.__name__, actor_id, e))
continue
def _ego_vehicle_control_applied_callback(self, ego_vehicle_id):
if not self.carla_settings.synchronous_mode or \
not self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
return
with self._expected_ego_vehicle_control_command_ids_lock:
if ego_vehicle_id in self._expected_ego_vehicle_control_command_ids:
self._expected_ego_vehicle_control_command_ids.remove(
ego_vehicle_id)
else:
rospy.logwarn(
"Unexpected vehicle control command received from {}".
format(ego_vehicle_id))
if not self._expected_ego_vehicle_control_command_ids:
self._all_vehicle_control_commands_received.set()
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
# check CARLA version
dist = pkg_resources.get_distribution("carla")
if LooseVersion(dist.version) < LooseVersion(self.CARLA_VERSION):
raise ImportError(
"CARLA version {} or newer required. CARLA version found: {}".
format(self.CARLA_VERSION, dist))
self.parameters = params
self.actors = {}
self.pseudo_actors = []
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
# set carla world settings
self.carla_settings = carla_world.get_settings()
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.update_actors_queue = queue.Queue(maxsize=1)
# start thread to update actors
self.update_actor_thread = Thread(
target=self._update_actors_thread)
self.update_actor_thread.start()
# create initially existing actors
self.update_actors_queue.put(
set([x.id for x in self.carla_world.get_snapshot()]))
# wait for first actors creation to be finished
self.update_actors_queue.join()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# add world info
self.pseudo_actors.append(
WorldInfo(carla_world=self.carla_world, communication=self.comm))
# add global object sensor
self.pseudo_actors.append(
ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
self.debug_helper = DebugHelper(carla_world.debug)
# add traffic light pseudo sensor
self.pseudo_actors.append(
TrafficLightsSensor(parent=None,
communication=self.comm,
actor_list=self.actors))
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
self.parameters = params
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
self.carla_settings = carla_world.get_settings()
if not self.parameters["passive"]:
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
# active sync mode in the ros bridge only if CARLA world is configured in sync mode and
# passive mode is not enabled.
self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters["passive"]
if self.carla_settings.synchronous_mode and self.parameters["passive"]:
rospy.loginfo(
"Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world.")
self.carla_control_queue = queue.Queue()
# actor factory
self.actor_factory = ActorFactory(self, carla_world, self.sync_mode)
# add world info
self.world_info = WorldInfo(carla_world=self.carla_world)
# add debug helper
self.debug_helper = DebugHelper(carla_world.debug)
# Communication topics
self.clock_publisher = rospy.Publisher('clock', Clock, queue_size=10)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.sync_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.actor_factory.start()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# services configuration.
self._registered_actors = []
self.spawn_object_service = rospy.Service("/carla/spawn_object", SpawnObject,
self.spawn_object)
self.destroy_object_service = rospy.Service("/carla/destroy_object", DestroyObject,
self.destroy_object)
self.get_blueprints_service = rospy.Service("/carla/get_blueprints", GetBlueprints,
self.get_blueprints)
self.carla_weather_subscriber = \
rospy.Subscriber("/carla/weather_control",
CarlaWeatherParameters, self.on_weather_changed)
class CarlaRosBridge(object):
"""
Carla Ros bridge
"""
CARLA_VERSION = "0.9.10"
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
self.parameters = params
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
self.carla_settings = carla_world.get_settings()
if not self.parameters["passive"]:
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
# active sync mode in the ros bridge only if CARLA world is configured in sync mode and
# passive mode is not enabled.
self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters["passive"]
if self.carla_settings.synchronous_mode and self.parameters["passive"]:
rospy.loginfo(
"Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world.")
self.carla_control_queue = queue.Queue()
# actor factory
self.actor_factory = ActorFactory(self, carla_world, self.sync_mode)
# add world info
self.world_info = WorldInfo(carla_world=self.carla_world)
# add debug helper
self.debug_helper = DebugHelper(carla_world.debug)
# Communication topics
self.clock_publisher = rospy.Publisher('clock', Clock, queue_size=10)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.sync_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.actor_factory.start()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# services configuration.
self._registered_actors = []
self.spawn_object_service = rospy.Service("/carla/spawn_object", SpawnObject,
self.spawn_object)
self.destroy_object_service = rospy.Service("/carla/destroy_object", DestroyObject,
self.destroy_object)
self.get_blueprints_service = rospy.Service("/carla/get_blueprints", GetBlueprints,
self.get_blueprints)
self.carla_weather_subscriber = \
rospy.Subscriber("/carla/weather_control",
CarlaWeatherParameters, self.on_weather_changed)
def _spawn_actor(self, req):
if "*" in req.type:
blueprint = secure_random.choice(
self.carla_world.get_blueprint_library().filter(req.type))
else:
blueprint = self.carla_world.get_blueprint_library().find(req.type)
blueprint.set_attribute('role_name', req.id)
for attribute in req.attributes:
blueprint.set_attribute(attribute.key, attribute.value)
if req.random_pose is False:
transform = trans.ros_pose_to_carla_transform(req.transform)
else:
# get a random pose
spawn_points = self.carla_world.get_map().get_spawn_points()
transform = secure_random.choice(
spawn_points) if spawn_points else carla.Transform()
attach_to = None
if req.attach_to != 0:
attach_to = self.carla_world.get_actor(req.attach_to)
if attach_to is None:
raise IndexError("Parent actor {} not found".format(req.attach_to))
carla_actor = self.carla_world.spawn_actor(blueprint, transform, attach_to)
actor = self.actor_factory.create(
req.type, req.id, req.attach_to, req.transform, carla_actor)
return actor.uid
def _spawn_pseudo_actor(self, req):
actor = self.actor_factory.create(req.type, req.id, req.attach_to, req.transform)
return actor.uid
def spawn_object(self, req):
with self.actor_factory.spawn_lock:
try:
if "pseudo" in req.type:
id_ = self._spawn_pseudo_actor(req)
else:
id_ = self._spawn_actor(req)
self._registered_actors.append(id_)
return SpawnObjectResponse(id_, "")
except Exception as e:
rospy.logwarn("Error spawning object '{}: {}".format(req.type, e))
return SpawnObjectResponse(-1, str(e))
def _destroy_actor(self, uid):
if uid not in self.actor_factory.actors:
return False
# remove actors that have the actor to be removed as parent.
for actor in list(self.actor_factory.actors.values()):
if actor.parent is not None and actor.parent.uid == uid:
if actor.uid in self._registered_actors:
success = self._destroy_actor(actor.uid)
if not success:
return False
actor = self.actor_factory.actors[uid]
if isinstance(actor, Actor):
actor.carla_actor.destroy()
self.actor_factory.destroy(uid)
if uid in self._registered_actors:
self._registered_actors.remove(uid)
return True
def destroy_object(self, req):
with self.actor_factory.spawn_lock:
result = self._destroy_actor(req.id)
return DestroyObjectResponse(result)
def get_blueprints(self, req):
response = GetBlueprintsResponse()
if req.filter:
bp_filter = req.filter
else:
bp_filter = "*"
response.blueprints = [
bp.id for bp in self.carla_world.get_blueprint_library().filter(bp_filter)]
response.blueprints.extend(self.actor_factory.get_pseudo_sensor_types())
response.blueprints.sort()
return response
def on_weather_changed(self, weather_parameters):
"""
Callback on new weather parameters
:return:
"""
if not self.carla_world:
return
rospy.loginfo("Applying weather parameters...")
weather = carla.WeatherParameters()
weather.cloudiness = weather_parameters.cloudiness
weather.precipitation = weather_parameters.precipitation
weather.precipitation_deposits = weather_parameters.precipitation_deposits
weather.wind_intensity = weather_parameters.wind_intensity
weather.fog_density = weather_parameters.fog_density
weather.fog_distance = weather_parameters.fog_distance
weather.wetness = weather_parameters.wetness
weather.sun_azimuth_angle = weather_parameters.sun_azimuth_angle
weather.sun_altitude_angle = weather_parameters.sun_altitude_angle
self.carla_world.set_weather(weather)
def process_run_state(self):
"""
process state changes
"""
command = None
# get last command
while not self.carla_control_queue.empty():
command = self.carla_control_queue.get()
while command is not None and not rospy.is_shutdown():
self.carla_run_state = command
if self.carla_run_state == CarlaControl.PAUSE:
# wait for next command
rospy.loginfo("State set to PAUSED")
self.status_publisher.set_synchronous_mode_running(False)
command = self.carla_control_queue.get()
elif self.carla_run_state == CarlaControl.PLAY:
rospy.loginfo("State set to PLAY")
self.status_publisher.set_synchronous_mode_running(True)
return
elif self.carla_run_state == CarlaControl.STEP_ONCE:
rospy.loginfo("Execute single step.")
self.status_publisher.set_synchronous_mode_running(True)
self.carla_control_queue.put(CarlaControl.PAUSE)
return
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
while not self.shutdown.is_set():
# Oscar: set start time
start_time = time.time()
self.process_run_state()
if self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
# fill list of available ego vehicles
self._expected_ego_vehicle_control_command_ids = []
with self._expected_ego_vehicle_control_command_ids_lock:
for actor_id, actor in self.actor_factory.actors.items():
if isinstance(actor, EgoVehicle):
self._expected_ego_vehicle_control_command_ids.append(
actor_id)
frame = self.carla_world.tick()
world_snapshot = self.carla_world.get_snapshot()
self.status_publisher.set_frame(frame)
self.update_clock(world_snapshot.timestamp)
rospy.logdebug("Tick for frame {} returned. Waiting for sensor data...".format(
frame))
start_time_sensors = time.time()
with self.actor_factory.lock:
self._update(frame, world_snapshot.timestamp.elapsed_seconds)
end_time = time.time()
duration = end_time - start_time
print('Sensor updates took ' + str(duration) + ' s')
rospy.logdebug("Waiting for sensor data finished.")
self.actor_factory.update()
if self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
# wait for all ego vehicles to send a vehicle control command
if self._expected_ego_vehicle_control_command_ids:
if not self._all_vehicle_control_commands_received.wait(1):
rospy.logwarn("Timeout (1s) while waiting for vehicle control commands. "
"Missing command from actor ids {}".format(
self._expected_ego_vehicle_control_command_ids))
self._all_vehicle_control_commands_received.clear()
# Oscar: Get the duration of the loop
end_time = time.time()
duration = end_time - start_time
# Oscar: Get a pointer to the current thread
event = threading.Event()
# Oscar: If our execution is fast enough, wait for real-time to catch up
if duration < self.carla_settings.fixed_delta_seconds:
# print('Waiting ' + str(self.carla_settings.fixed_delta_seconds - duration) + ' s')
event.wait(self.carla_settings.fixed_delta_seconds - duration)
else:
rospy.logwarn('Warning: Carla ROS Bridge is currently too slow to run in real-time!')
def _carla_time_tick(self, carla_snapshot):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates.
After successful locking the update mutex
(only perform trylock to respect bridge processing time)
the clock and the children are updated.
Finally the ROS messages collected to be published are sent out.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
print('carla time tick')
if not self.shutdown.is_set():
if self.actor_factory.lock.acquire(False):
if self.timestamp_last_run < carla_snapshot.timestamp.elapsed_seconds:
self.timestamp_last_run = carla_snapshot.timestamp.elapsed_seconds
self.update_clock(carla_snapshot.timestamp)
self.status_publisher.set_frame(carla_snapshot.frame)
self._update(carla_snapshot.frame,
carla_snapshot.timestamp.elapsed_seconds)
self.actor_factory.lock.release()
def run(self):
"""
Run the bridge functionality.
Registers on shutdown callback at rospy and spins ROS.
:return:
"""
rospy.on_shutdown(self.on_shutdown)
rospy.spin()
def on_shutdown(self):
"""
Function to be called on shutdown.
This function is registered at rospy as shutdown handler.
"""
rospy.loginfo("Shutdown requested")
self.destroy()
def _update(self, frame_id, timestamp):
"""
update all actors
:return:
"""
# update world info
self.world_info.update(frame_id, timestamp)
# update all carla actors
for actor_id in self.actor_factory.actors:
try:
self.actor_factory.actors[actor_id].update(frame_id, timestamp)
except RuntimeError as e:
rospy.logwarn("Update actor {}({}) failed: {}".format(
self.actor_factory.actors[actor_id].__class__.__name__, actor_id, e))
continue
def _ego_vehicle_control_applied_callback(self, ego_vehicle_id):
if not self.sync_mode or \
not self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
return
with self._expected_ego_vehicle_control_command_ids_lock:
if ego_vehicle_id in self._expected_ego_vehicle_control_command_ids:
self._expected_ego_vehicle_control_command_ids.remove(
ego_vehicle_id)
else:
rospy.logwarn(
"Unexpected vehicle control command received from {}".format(ego_vehicle_id))
if not self._expected_ego_vehicle_control_command_ids:
self._all_vehicle_control_commands_received.set()
def update_clock(self, carla_timestamp):
"""
perform the update of the clock
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
self.ros_timestamp = rospy.Time.from_sec(
carla_timestamp.elapsed_seconds)
self.clock_publisher.publish(Clock(self.ros_timestamp))
def destroy(self):
"""
Function to destroy this object.
:return:
"""
rospy.signal_shutdown("")
self.debug_helper.destroy()
self.shutdown.set()
self.carla_weather_subscriber.unregister()
self.carla_control_queue.put(CarlaControl.STEP_ONCE)
if not self.sync_mode:
if self.on_tick_id:
self.carla_world.remove_on_tick(self.on_tick_id)
self.actor_factory.thread.join()
with self.actor_factory.spawn_lock:
# remove actors in reverse order to destroy parent actors first.
for uid in self._registered_actors[::-1]:
self._destroy_actor(uid)
self.actor_factory.clear()
rospy.loginfo("Exiting Bridge")
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
self.parameters = params
self.actors = {}
self.pseudo_actors = []
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
# set carla world settings
self.carla_settings = carla_world.get_settings()
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
rospy.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"]
rospy.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
# Communication topics
self.clock_publisher = rospy.Publisher('clock', Clock, queue_size=10)
self.tf_publisher = rospy.Publisher('tf', TFMessage, queue_size=100)
self.marker_publisher = rospy.Publisher('/carla/marker',
Marker,
queue_size=10)
self.actor_list_publisher = rospy.Publisher('/carla/actor_list',
CarlaActorList,
queue_size=10,
latch=True)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
rospy.Subscriber("/carla/control", CarlaControl,
lambda control: self.carla_control_queue.put(control.command))
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.update_actors_queue = queue.Queue(maxsize=1)
# start thread to update actors
self.update_actor_thread = Thread(
target=self._update_actors_thread)
self.update_actor_thread.start()
# create initially existing actors
self.update_actors_queue.put(
set([x.id for x in self.carla_world.get_snapshot()]))
# wait for first actors creation to be finished
self.update_actors_queue.join()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
self.carla_weather_subscriber = \
rospy.Subscriber("/carla/weather_control",
CarlaWeatherParameters, self.on_weather_changed)
# add world info
self.pseudo_actors.append(WorldInfo(carla_world=self.carla_world,
node=self))
# add global object sensor
self.pseudo_actors.append(ObjectSensor(parent=None,
node=self,
actor_list=self.actors,
filtered_id=None))
self.debug_helper = DebugHelper(carla_world.debug)
# add traffic light pseudo sensor
self.pseudo_actors.append(TrafficLightsSensor(parent=None,
node=self,
actor_list=self.actors))
def initialize_bridge(self, carla_world, params):
"""
Initialize the bridge
"""
self.parameters = params
self.carla_world = carla_world
self.ros_timestamp = roscomp.ros_timestamp()
self.callback_group = roscomp.callback_groups.ReentrantCallbackGroup()
self.synchronous_mode_update_thread = None
self.shutdown = Event()
self.carla_settings = carla_world.get_settings()
if not self.parameters["passive"]:
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
self.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
self.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.loginfo("Parameters:")
for key in self.parameters:
self.loginfo(" {}: {}".format(key, self.parameters[key]))
# active sync mode in the ros bridge only if CARLA world is configured in sync mode and
# passive mode is not enabled.
self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters[
"passive"]
if self.carla_settings.synchronous_mode and self.parameters["passive"]:
self.loginfo(
"Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world."
)
self.carla_control_queue = queue.Queue()
# actor factory
self.actor_factory = ActorFactory(self, carla_world, self.sync_mode)
# add world info
self.world_info = WorldInfo(carla_world=self.carla_world, node=self)
# add debug helper
self.debug_helper = DebugHelper(carla_world.debug, self)
# Communication topics
self.clock_publisher = self.new_publisher(Clock, 'clock', 10)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds, self)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.sync_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
self.new_subscription(CarlaControl, "/carla/control",
lambda control: self.carla_control_queue.put(control.command),
qos_profile=10, callback_group=self.callback_group)
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.actor_factory.start()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# services configuration.
self._registered_actors = []
self.spawn_object_service = self.new_service(SpawnObject,
"/carla/spawn_object",
self.spawn_object)
self.destroy_object_service = self.new_service(
DestroyObject, "/carla/destroy_object", self.destroy_object)
self.get_blueprints_service = self.new_service(
GetBlueprints,
"/carla/get_blueprints",
self.get_blueprints,
callback_group=self.callback_group)
self.carla_weather_subscriber = \
self.new_subscription(CarlaWeatherParameters, "/carla/weather_control",
self.on_weather_changed, qos_profile=10, callback_group=self.callback_group)
class CarlaRosBridge(CompatibleNode):
"""
Carla Ros bridge
"""
with open(os.path.join(os.path.dirname(__file__), "CARLA_VERSION")) as f:
CARLA_VERSION = f.read()[:-1]
# in synchronous mode, if synchronous_mode_wait_for_vehicle_control_command is True,
# wait for this time until a next tick is triggered.
VEHICLE_CONTROL_TIMEOUT = 1.
def __init__(self):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
super(CarlaRosBridge, self).__init__("ros_bridge_node")
# pylint: disable=attribute-defined-outside-init
def initialize_bridge(self, carla_world, params):
"""
Initialize the bridge
"""
self.parameters = params
self.carla_world = carla_world
self.ros_timestamp = roscomp.ros_timestamp()
self.callback_group = roscomp.callback_groups.ReentrantCallbackGroup()
self.synchronous_mode_update_thread = None
self.shutdown = Event()
self.carla_settings = carla_world.get_settings()
if not self.parameters["passive"]:
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
self.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
self.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.loginfo("Parameters:")
for key in self.parameters:
self.loginfo(" {}: {}".format(key, self.parameters[key]))
# active sync mode in the ros bridge only if CARLA world is configured in sync mode and
# passive mode is not enabled.
self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters[
"passive"]
if self.carla_settings.synchronous_mode and self.parameters["passive"]:
self.loginfo(
"Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world."
)
self.carla_control_queue = queue.Queue()
# actor factory
self.actor_factory = ActorFactory(self, carla_world, self.sync_mode)
# add world info
self.world_info = WorldInfo(carla_world=self.carla_world, node=self)
# add debug helper
self.debug_helper = DebugHelper(carla_world.debug, self)
# Communication topics
self.clock_publisher = self.new_publisher(Clock, 'clock', 10)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds, self)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.sync_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
self.new_subscription(CarlaControl, "/carla/control",
lambda control: self.carla_control_queue.put(control.command),
qos_profile=10, callback_group=self.callback_group)
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.actor_factory.start()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# services configuration.
self._registered_actors = []
self.spawn_object_service = self.new_service(SpawnObject,
"/carla/spawn_object",
self.spawn_object)
self.destroy_object_service = self.new_service(
DestroyObject, "/carla/destroy_object", self.destroy_object)
self.get_blueprints_service = self.new_service(
GetBlueprints,
"/carla/get_blueprints",
self.get_blueprints,
callback_group=self.callback_group)
self.carla_weather_subscriber = \
self.new_subscription(CarlaWeatherParameters, "/carla/weather_control",
self.on_weather_changed, qos_profile=10, callback_group=self.callback_group)
def spawn_object(self, req, response=None):
response = roscomp.get_service_response(SpawnObject)
if not self.shutdown.is_set():
try:
id_ = self.actor_factory.spawn_actor(req)
self._registered_actors.append(id_)
response.id = id_
except Exception as e:
self.logwarn("Error spawning object '{}': {}".format(
req.type, e))
response.id = -1
response.error_string = str(e)
else:
response.id = -1
response.error_string = 'Bridge is shutting down, object will not be spawned.'
return response
def destroy_object(self, req, response=None):
response = roscomp.get_service_response(DestroyObject)
destroyed_actors = self.actor_factory.destroy_actor(req.id)
response.success = bool(destroyed_actors)
for actor in destroyed_actors:
if actor in self._registered_actors:
self._registered_actors.remove(actor)
return response
def get_blueprints(self, req):
response = roscomp.get_service_response(GetBlueprints)
if req.filter:
bp_filter = req.filter
else:
bp_filter = "*"
response.blueprints = [
bp.id for bp in self.carla_world.get_blueprint_library().filter(
bp_filter)
]
response.blueprints.extend(
self.actor_factory.get_pseudo_sensor_types())
response.blueprints.sort()
return response
def on_weather_changed(self, weather_parameters):
"""
Callback on new weather parameters
:return:
"""
if not self.carla_world:
return
self.loginfo("Applying weather parameters...")
weather = carla.WeatherParameters()
weather.cloudiness = weather_parameters.cloudiness
weather.precipitation = weather_parameters.precipitation
weather.precipitation_deposits = weather_parameters.precipitation_deposits
weather.wind_intensity = weather_parameters.wind_intensity
weather.fog_density = weather_parameters.fog_density
weather.fog_distance = weather_parameters.fog_distance
weather.wetness = weather_parameters.wetness
weather.sun_azimuth_angle = weather_parameters.sun_azimuth_angle
weather.sun_altitude_angle = weather_parameters.sun_altitude_angle
self.carla_world.set_weather(weather)
def process_run_state(self):
"""
process state changes
"""
command = None
# get last command
while not self.carla_control_queue.empty():
command = self.carla_control_queue.get()
while command is not None and roscomp.ok():
self.carla_run_state = command
if self.carla_run_state == CarlaControl.PAUSE:
# wait for next command
self.loginfo("State set to PAUSED")
self.status_publisher.set_synchronous_mode_running(False)
command = self.carla_control_queue.get()
elif self.carla_run_state == CarlaControl.PLAY:
self.loginfo("State set to PLAY")
self.status_publisher.set_synchronous_mode_running(True)
return
elif self.carla_run_state == CarlaControl.STEP_ONCE:
self.loginfo("Execute single step.")
self.status_publisher.set_synchronous_mode_running(True)
self.carla_control_queue.put(CarlaControl.PAUSE)
return
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
while not self.shutdown.is_set() and roscomp.ok():
self.process_run_state()
if self.parameters[
'synchronous_mode_wait_for_vehicle_control_command']:
# fill list of available ego vehicles
self._expected_ego_vehicle_control_command_ids = []
with self._expected_ego_vehicle_control_command_ids_lock:
for actor_id, actor in self.actor_factory.actors.items():
if isinstance(actor, EgoVehicle):
self._expected_ego_vehicle_control_command_ids.append(
actor_id)
self.actor_factory.update_available_objects()
frame = self.carla_world.tick()
world_snapshot = self.carla_world.get_snapshot()
self.status_publisher.set_frame(frame)
self.update_clock(world_snapshot.timestamp)
self.logdebug(
"Tick for frame {} returned. Waiting for sensor data...".
format(frame))
self._update(frame, world_snapshot.timestamp.elapsed_seconds)
self.logdebug("Waiting for sensor data finished.")
if self.parameters[
'synchronous_mode_wait_for_vehicle_control_command']:
# wait for all ego vehicles to send a vehicle control command
if self._expected_ego_vehicle_control_command_ids:
if not self._all_vehicle_control_commands_received.wait(
CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT):
self.logwarn(
"Timeout ({}s) while waiting for vehicle control commands. "
"Missing command from actor ids {}".format(
CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT, self.
_expected_ego_vehicle_control_command_ids))
self._all_vehicle_control_commands_received.clear()
def _carla_time_tick(self, carla_snapshot):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates.
After successful locking the update mutex
(only perform trylock to respect bridge processing time)
the clock and the children are updated.
Finally the ROS messages collected to be published are sent out.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if not self.shutdown.is_set():
if self.timestamp_last_run < carla_snapshot.timestamp.elapsed_seconds:
self.timestamp_last_run = carla_snapshot.timestamp.elapsed_seconds
self.update_clock(carla_snapshot.timestamp)
self.status_publisher.set_frame(carla_snapshot.frame)
self._update(carla_snapshot.frame,
carla_snapshot.timestamp.elapsed_seconds)
def _update(self, frame_id, timestamp):
"""
update all actors
:return:
"""
self.world_info.update(frame_id, timestamp)
self.actor_factory.update_actor_states(frame_id, timestamp)
def _ego_vehicle_control_applied_callback(self, ego_vehicle_id):
if not self.sync_mode or \
not self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
return
with self._expected_ego_vehicle_control_command_ids_lock:
if ego_vehicle_id in self._expected_ego_vehicle_control_command_ids:
self._expected_ego_vehicle_control_command_ids.remove(
ego_vehicle_id)
else:
self.logwarn(
"Unexpected vehicle control command received from {}".
format(ego_vehicle_id))
if not self._expected_ego_vehicle_control_command_ids:
self._all_vehicle_control_commands_received.set()
def update_clock(self, carla_timestamp):
"""
perform the update of the clock
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if roscomp.ok():
self.ros_timestamp = roscomp.ros_timestamp(
carla_timestamp.elapsed_seconds, from_sec=True)
self.clock_publisher.publish(Clock(clock=self.ros_timestamp))
def destroy(self):
"""
Function to destroy this object.
:return:
"""
self.loginfo("Shutting down...")
self.shutdown.set()
if not self.sync_mode:
if self.on_tick_id:
self.carla_world.remove_on_tick(self.on_tick_id)
self.actor_factory.thread.join()
else:
self.synchronous_mode_update_thread.join()
self.loginfo("Object update finished.")
self.debug_helper.destroy()
self.status_publisher.destroy()
self.destroy_service(self.spawn_object_service)
self.destroy_service(self.destroy_object_service)
self.destroy_subscription(self.carla_weather_subscriber)
self.carla_control_queue.put(CarlaControl.STEP_ONCE)
for uid in self._registered_actors:
self.actor_factory.destroy_actor(uid)
self.actor_factory.update_available_objects()
self.actor_factory.clear()
super(CarlaRosBridge, self).destroy()
