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()