def run(self):
"""
Control loop
:return:
"""
current_req = None
while roscomp.ok():
if current_req:
if self._scenario_runner.is_running():
self.loginfo("Scenario Runner currently running. Shutting down.")
self._scenario_runner.shutdown()
self.loginfo("Scenario Runner stopped.")
self.loginfo("Executing scenario {}...".format(current_req.name))
# execute scenario
scenario_executed = self._scenario_runner.execute_scenario(
current_req.scenario_file)
if not scenario_executed:
self.logwarn("Unable to execute scenario.")
current_req = None
else:
try:
current_req = self._request_queue.get(block=True, timeout=0.5)
except queue.Empty:
# no new request
pass
if self._scenario_runner.is_running():
self.loginfo("Scenario Runner currently running. Shutting down.")
self._scenario_runner.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("ad_agent", args=args)
controller = None
try:
executor = roscomp.executors.MultiThreadedExecutor()
controller = CarlaAdAgent()
executor.add_node(controller)
roscomp.on_shutdown(controller.emergency_stop)
update_timer = controller.new_timer(
0.05, lambda timer_event=None: controller.run_step())
controller.spin()
except (ROSInterruptException, ROSException) as e:
if roscomp.ok():
roscomp.logwarn("ROS Error during exection: {}".format(e))
except KeyboardInterrupt:
roscomp.loginfo("User requested shut down.")
finally:
roscomp.shutdown()
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 twist_received(self, twist):
"""
receive twist and convert to carla vehicle control
"""
if self.max_steering_angle is None:
self.logwarn("Did not yet receive vehicle info.")
return
control = CarlaEgoVehicleControl()
if twist == Twist():
# stop
control.throttle = 0.
control.brake = 1.
control.steer = 0.
else:
if twist.linear.x > 0:
control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION,
twist.linear.x) / TwistToVehicleControl.MAX_LON_ACCELERATION
else:
control.reverse = True
control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION,
twist.linear.x) / -TwistToVehicleControl.MAX_LON_ACCELERATION
if twist.angular.z > 0:
control.steer = -min(self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
else:
control.steer = -max(-self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
try:
self.pub.publish(control)
except ROSException as e:
if roscomp.ok():
self.logwarn("Error while publishing control: {}".format(e))
def _update_synchronous_sensor(self, frame, timestamp):
while not self.next_data_expected_time or \
(not self.queue.empty() or
self.next_data_expected_time and
self.next_data_expected_time < timestamp):
while True:
try:
carla_sensor_data = self.queue.get(timeout=1.0)
if carla_sensor_data.frame == frame:
self.node.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_tf(
trans.carla_transform_to_ros_pose(
carla_sensor_data.transform), timestamp)
self.sensor_data_updated(carla_sensor_data)
return
elif carla_sensor_data.frame < frame:
self.node.logwarn(
"{}({}): skipping old frame {}, expected {}".
format(self.__class__.__name__, self.get_id(),
carla_sensor_data.frame, frame))
except queue.Empty:
if roscomp.ok():
self.node.logwarn(
"{}({}): Expected Frame {} not received".format(
self.__class__.__name__, self.get_id(), frame))
return
def _callback_sensor_data(self, carla_sensor_data):
"""
Callback function called whenever new sensor data is received
:param carla_sensor_data: carla sensor data object
:type carla_sensor_data: carla.SensorData
"""
if not self._callback_active.acquire(False):
# if acquire fails, sensor is currently getting destroyed
return
if self.synchronous_mode:
if self.sensor_tick_time:
self.next_data_expected_time = carla_sensor_data.timestamp + \
float(self.sensor_tick_time)
self.queue.put(carla_sensor_data)
else:
self.publish_tf(
trans.carla_transform_to_ros_pose(carla_sensor_data.transform),
carla_sensor_data.timestamp)
try:
self.sensor_data_updated(carla_sensor_data)
except roscomp.exceptions.ROSException:
if roscomp.ok():
self.node.logwarn(
"Sensor {}: Error while executing sensor_data_updated()."
.format(self.uid))
self._callback_active.release()
def publish_tf(self, pose, timestamp):
transform = self.get_ros_transform(pose, timestamp)
try:
self._tf_broadcaster.sendTransform(transform)
except roscomp.exceptions.ROSException:
if roscomp.ok():
self.node.logwarn("Sensor {} failed to send transform.".format(
self.uid))
def get_waypoint(self, location):
"""
Helper method to get a waypoint for a location.
:param location: location request
:type location: geometry_msgs/Point
:return: waypoint of the requested location
:rtype: carla_msgs/Waypoint
"""
if not roscomp.ok():
return None
try:
request = roscomp.get_service_request(GetWaypoint)
request.location = location
response = self.call_service(self._get_waypoint_client, request)
return response.waypoint
except ServiceException as e:
if roscomp.ok():
self.logwarn("Service call 'get_waypoint' failed: {}".format(
str(e)))
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 _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 main(args=None):
"""
main function
"""
roscomp.init("manual_control", args=args)
# resolution should be similar to spawned camera with role-name 'view'
resolution = {"width": 800, "height": 600}
pygame.init()
pygame.font.init()
pygame.display.set_caption("CARLA ROS manual control")
try:
display = pygame.display.set_mode(
(resolution['width'], resolution['height']),
pygame.HWSURFACE | pygame.DOUBLEBUF)
manual_control_node = ManualControl(resolution)
clock = pygame.time.Clock()
executor = roscomp.executors.MultiThreadedExecutor()
executor.add_node(manual_control_node)
spin_thread = Thread(target=manual_control_node.spin)
spin_thread.start()
while roscomp.ok():
clock.tick_busy_loop(60)
if manual_control_node.render(clock, display):
return
pygame.display.flip()
except KeyboardInterrupt:
roscomp.loginfo("User requested shut down.")
finally:
roscomp.shutdown()
spin_thread.join()
pygame.quit()
def setup_sensors(self, sensors, attached_vehicle_id=None):
"""
Create the sensors defined by the user and attach them to the vehicle
(or not if global sensor)
:param sensors: list of sensors
:param attached_vehicle_id: id of vehicle to attach the sensors to
:return actors: list of ids of objects created
"""
sensor_names = []
for sensor_spec in sensors:
if not roscomp.ok():
break
try:
sensor_type = str(sensor_spec.pop("type"))
sensor_id = str(sensor_spec.pop("id"))
sensor_name = sensor_type + "/" + sensor_id
if sensor_name in sensor_names:
raise NameError
sensor_names.append(sensor_name)
if attached_vehicle_id is None and "pseudo" not in sensor_type:
spawn_point = sensor_spec.pop("spawn_point")
sensor_transform = self.create_spawn_point(
spawn_point.pop("x"), spawn_point.pop("y"),
spawn_point.pop("z"), spawn_point.pop("roll", 0.0),
spawn_point.pop("pitch", 0.0),
spawn_point.pop("yaw", 0.0))
else:
# if sensor attached to a vehicle, or is a 'pseudo_actor', allow default pose
spawn_point = sensor_spec.pop("spawn_point", 0)
if spawn_point == 0:
sensor_transform = self.create_spawn_point(
0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
sensor_transform = self.create_spawn_point(
spawn_point.pop("x",
0.0), spawn_point.pop("y", 0.0),
spawn_point.pop("z", 0.0),
spawn_point.pop("roll", 0.0),
spawn_point.pop("pitch", 0.0),
spawn_point.pop("yaw", 0.0))
spawn_object_request = roscomp.get_service_request(SpawnObject)
spawn_object_request.type = sensor_type
spawn_object_request.id = sensor_id
spawn_object_request.attach_to = attached_vehicle_id if attached_vehicle_id is not None else 0
spawn_object_request.transform = sensor_transform
spawn_object_request.random_pose = False # never set a random pose for a sensor
attached_objects = []
for attribute, value in sensor_spec.items():
if attribute == "attached_objects":
for attached_object in sensor_spec["attached_objects"]:
attached_objects.append(attached_object)
continue
spawn_object_request.attributes.append(
KeyValue(key=str(attribute), value=str(value)))
response_id = self.spawn_object(spawn_object_request)
if response_id == -1:
raise RuntimeError(response.error_string)
if attached_objects:
# spawn the attached objects
self.setup_sensors(attached_objects, response_id)
if attached_vehicle_id is None:
self.global_sensors.append(response_id)
else:
self.vehicles_sensors.append(response_id)
except KeyError as e:
self.logerr(
"Sensor {} will not be spawned, the mandatory attribute {} is missing"
.format(sensor_name, e))
continue
except RuntimeError as e:
self.logerr("Sensor {} will not be spawned: {}".format(
sensor_name, e))
continue
except NameError:
self.logerr(
"Sensor rolename '{}' is only allowed to be used once. The second one will be ignored."
.format(sensor_id))
continue
def setup_vehicles(self, vehicles):
for vehicle in vehicles:
if self.spawn_sensors_only is True:
# spawn sensors of already spawned vehicles
try:
carla_id = vehicle["carla_id"]
except KeyError as e:
self.logerr(
"Could not spawn sensors of vehicle {}, its carla ID is not known."
.format(vehicle["id"]))
break
# spawn the vehicle's sensors
self.setup_sensors(vehicle["sensors"], carla_id)
else:
spawn_object_request = roscomp.get_service_request(SpawnObject)
spawn_object_request.type = vehicle["type"]
spawn_object_request.id = vehicle["id"]
spawn_object_request.attach_to = 0
spawn_object_request.random_pose = False
spawn_point = None
# check if there's a spawn_point corresponding to this vehicle
spawn_point_param = self.get_param(
"spawn_point_" + vehicle["id"], None)
spawn_param_used = False
if (spawn_point_param is not None):
# try to use spawn_point from parameters
spawn_point = self.check_spawn_point_param(
spawn_point_param)
if spawn_point is None:
self.logwarn(
"{}: Could not use spawn point from parameters, ".
format(vehicle["id"]) +
"the spawn point from config file will be used.")
else:
self.loginfo("Spawn point from ros parameters")
spawn_param_used = True
if "spawn_point" in vehicle and spawn_param_used is False:
# get spawn point from config file
try:
spawn_point = self.create_spawn_point(
vehicle["spawn_point"]["x"],
vehicle["spawn_point"]["y"],
vehicle["spawn_point"]["z"],
vehicle["spawn_point"]["roll"],
vehicle["spawn_point"]["pitch"],
vehicle["spawn_point"]["yaw"])
self.loginfo("Spawn point from configuration file")
except KeyError as e:
self.logerr(
"{}: Could not use the spawn point from config file, "
.format(vehicle["id"]) +
"the mandatory attribute {} is missing, a random spawn point will be used"
.format(e))
if spawn_point is None:
# pose not specified, ask for a random one in the service call
self.loginfo("Spawn point selected at random")
spawn_point = Pose() # empty pose
spawn_object_request.random_pose = True
player_spawned = False
while not player_spawned and roscomp.ok():
spawn_object_request.transform = spawn_point
response_id = self.spawn_object(spawn_object_request)
if response_id != -1:
player_spawned = True
self.players.append(response_id)
# Set up the sensors
try:
self.setup_sensors(vehicle["sensors"], response_id)
except KeyError:
self.logwarn(
"Object (type='{}', id='{}') has no 'sensors' field in his config file, none will be spawned."
.format(spawn_object_request.type,
spawn_object_request.id))