def main(args=None):
"""
main function
:return:
"""
ros_init(args)
controller = None
try:
controller = CarlaAdAgent()
while ros_ok():
time.sleep(0.01)
if ROS_VERSION == 2:
rclpy.spin_once(controller)
controller.run_step()
except (ROSInterruptException, ROSException) as e:
if ros_ok():
logwarn("ROS Error during exection: {}".format(e))
except KeyboardInterrupt:
loginfo("User requested shut down.")
finally:
if controller is not None:
stopping_speed = Float64()
stopping_speed.data = 0.0
controller.speed_command_publisher.publish(stopping_speed)
ros_shutdown()
def get_waypoint(self, location):
"""
Helper to get waypoint for location via ros service.
Only used if risk should be avoided.
"""
if not ros_ok():
return None
try:
response = self.node.call_service(self._get_waypoint_client,
location)
return response.waypoint
except ServiceException as e:
if ros_ok():
self.node.logwarn(
"Service call 'get_waypoint' failed: {}".format(str(e)))
def run(self):
"""
Control loop
:return:
"""
current_req = None
while ros_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:
"""
ros_init(args)
controller = None
try:
executor = MultiThreadedExecutor()
controller = CarlaAdAgent()
executor.add_node(controller)
update_timer = controller.new_timer(
0.05, lambda timer_event=None: controller.run_step())
controller.spin()
except (ROSInterruptException, ROSException) as e:
if ros_ok():
logwarn("ROS Error during exection: {}".format(e))
except KeyboardInterrupt:
loginfo("User requested shut down.")
finally:
if controller is not None:
stopping_speed = Float64()
stopping_speed.data = 0.0
controller.speed_command_publisher.publish(stopping_speed)
ros_shutdown()
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 ROSException:
if ros_ok():
self.node.logwarn(
"Sensor {}: Error while executing sensor_data_updated()."
.format(self.uid))
self._callback_active.release()
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 ros_ok():
self.node.logwarn(
"{}({}): Expected Frame {} not received".format(
self.__class__.__name__, self.get_id(), frame))
return
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 ros_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 publish_tf(self, pose, timestamp):
transform = self.get_ros_transform(pose, timestamp)
try:
self._tf_broadcaster.sendTransform(transform)
except ROSException:
if ros_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 ros_ok():
return None
try:
request = get_service_request(GetWaypoint)
request.location = location
response = self.call_service(self._get_waypoint_client, request)
return response.waypoint
except ServiceException as e:
if ros_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 ros_ok():
self.ros_timestamp = ros_timestamp(carla_timestamp.elapsed_seconds,
from_sec=True)
self.clock_publisher.publish(Clock(clock=self.ros_timestamp))
def get_actor_waypoint(self, actor_id):
"""
helper method to get waypoint for actor via ros service
Only used if risk should be avoided.
"""
try:
request = get_service_request(GetActorWaypoint)
request.id = actor_id
response = self.node.call_service(self._get_actor_waypoint_client,
request)
return response.waypoint
except ServiceException as e:
if ros_ok():
self.node.logwarn("Service call failed: {}".format(e))
def run(self):
"""
Control loop
:return:
"""
loop_frequency = 20
if ROS_VERSION == 1:
r = rospy.Rate(loop_frequency)
self.loginfo("Starting run loop")
while ros_ok():
if self._waypoints:
control = CarlaWalkerControl()
direction = Vector3()
direction.x = self._waypoints[
0].position.x - self._current_pose.position.x
direction.y = self._waypoints[
0].position.y - self._current_pose.position.y
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
if direction_norm > CarlaWalkerAgent.MIN_DISTANCE:
control.speed = self._target_speed
control.direction.x = direction.x / direction_norm
control.direction.y = direction.y / direction_norm
else:
self._waypoints = self._waypoints[1:]
if self._waypoints:
self.loginfo("next waypoint: {} {}".format(
self._waypoints[0].position.x,
self._waypoints[0].position.y))
else:
self.loginfo("Route finished.")
self.control_publisher.publish(control)
try:
if ROS_VERSION == 1:
r.sleep()
elif ROS_VERSION == 2:
# TODO: use rclpy.Rate, not working yet
time.sleep(1 / loop_frequency)
except ROSInterruptException:
pass
def _is_light_red_europe_style(self, lights_list):
"""
This method is specialized to check European style traffic lights.
:param lights_list: list containing TrafficLight objects
:return: a tuple given by (bool_flag, traffic_light), where
- bool_flag is True if there is a traffic light in RED
affecting us and False otherwise
- traffic_light is the object itself or None if there is no
red traffic light affecting us
"""
if self._vehicle_location is None:
# no available self location yet
return (False, None)
ego_vehicle_location = get_service_request(GetWaypoint)
ego_vehicle_location.location = self._vehicle_location
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location)
if not ego_vehicle_waypoint:
if ros_ok():
self.node.logwarn("Could not get waypoint for ego vehicle.")
return (False, None)
for traffic_light in lights_list:
object_waypoint = traffic_light[1]
if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \
object_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(object_waypoint.pose.position,
ego_vehicle_location.location,
math.degrees(self._vehicle_yaw),
self._proximity_threshold):
traffic_light_state = CarlaTrafficLightStatus.RED
for status in self._traffic_lights:
if status.id == traffic_light[0]:
traffic_light_state = status.state
break
if traffic_light_state == CarlaTrafficLightStatus.RED:
return (True, traffic_light[0])
return (False, None)
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
while not self.shutdown.is_set() and ros_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)
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.")
self.actor_factory.update_available_objects()
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
"""
ros_init(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()
if ROS_VERSION == 2:
executer = rclpy.executors.MultiThreadedExecutor()
executer.add_node(manual_control_node)
spin_thread = Thread(target=executer.spin)
spin_thread.start()
while ros_ok():
clock.tick_busy_loop(60)
if manual_control_node.render(clock, display):
return
pygame.display.flip()
except KeyboardInterrupt:
loginfo("User requested shut down.")
finally:
ros_shutdown()
if ROS_VERSION == 2:
spin_thread.join()
pygame.quit()
def publish_tf(self, pose, timestamp):
if self.synchronous_mode:
if not self.relative_spawn_pose:
self.node.logwarn("{}: No relative spawn pose defined".format(
self.get_prefix()))
return
pose = self.relative_spawn_pose
child_frame_id = self.get_prefix()
if self.parent is not None:
frame_id = self.parent.get_prefix()
else:
frame_id = "map"
else:
child_frame_id = self.get_prefix()
frame_id = "map"
transform = tf2_ros.TransformStamped()
transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True)
transform.header.frame_id = frame_id
transform.child_frame_id = child_frame_id
transform.transform.translation.x = pose.position.x
transform.transform.translation.y = pose.position.y
transform.transform.translation.z = pose.position.z
transform.transform.rotation.x = pose.orientation.x
transform.transform.rotation.y = pose.orientation.y
transform.transform.rotation.z = pose.orientation.z
transform.transform.rotation.w = pose.orientation.w
try:
self._tf_broadcaster.sendTransform(transform)
except ROSException:
if ros_ok():
self.node.logwarn("Sensor {} failed to send transform.".format(
self.uid))
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 ros_ok():
self.logwarn("Error while publishing control: {}".format(e))
def _is_vehicle_hazard(self, vehicle_list, objects):
"""
Check if a given vehicle is an obstacle in our way. To this end we take
into account the road and lane the target vehicle is on and run a
geometry test to check if the target vehicle is under a certain distance
in front of our ego vehicle.
WARNING: This method is an approximation that could fail for very large
vehicles, which center is actually on a different lane but their
extension falls within the ego vehicle lane.
:param vehicle_list: list of potential obstacle to check
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
"""
if self._vehicle_location is None:
# no available self location yet
return (False, None)
ego_vehicle_location = get_service_request(GetWaypoint)
ego_vehicle_location.location = self._vehicle_location
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location)
if not ego_vehicle_waypoint:
if ros_ok():
self.node.logwarn("Could not get waypoint for ego vehicle.")
return (False, None)
for target_vehicle_id in vehicle_list:
# do not account for the ego vehicle
if target_vehicle_id == self._vehicle_id:
continue
target_vehicle_location = None
for elem in objects:
if elem.id == target_vehicle_id:
target_vehicle_location = elem.pose
break
if not target_vehicle_location:
self.node.logwarn("Location of vehicle {} not found".format(
target_vehicle_id))
continue
# if the object is not in our lane it's not an obstacle
request = get_service_request(GetWaypoint)
request.location = target_vehicle_location.position
target_vehicle_waypoint = self.get_waypoint(request)
if not target_vehicle_waypoint:
if ros_ok():
self.node.logwarn(
"Could not get waypoint for target vehicle.")
return (False, None)
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(target_vehicle_location.position,
self._vehicle_location,
math.degrees(self._vehicle_yaw),
self._proximity_threshold):
return (True, target_vehicle_id)
return (False, None)
def _is_light_red_us_style(self, lights_list): # pylint: disable=too-many-branches
"""
This method is specialized to check US style traffic lights.
:param lights_list: list containing TrafficLight objects
:return: a tuple given by (bool_flag, traffic_light), where
- bool_flag is True if there is a traffic light in RED
affecting us and False otherwise
- traffic_light is the object itself or None if there is no
red traffic light affecting us
"""
if self._vehicle_location is None:
# no available self location yet
return (False, None)
ego_vehicle_location = get_service_request(GetWaypoint)
ego_vehicle_location.location = self._vehicle_location
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location)
if not ego_vehicle_waypoint:
if ros_ok():
self.node.logwarn("Could not get waypoint for ego vehicle.")
return (False, None)
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
return (False, None)
if self._target_route_point is not None:
request = get_service_request(GetWaypoint)
request.location = self._target_route_point
target_waypoint = self.get_waypoint(request)
if not target_waypoint:
if ros_ok():
self.node.logwarn(
"Could not get waypoint for target route point.")
return (False, None)
if target_waypoint.is_junction:
min_angle = 180.0
sel_magnitude = 0.0 # pylint: disable=unused-variable
sel_traffic_light = None
for traffic_light in lights_list:
loc = traffic_light[1]
magnitude, angle = compute_magnitude_angle(
loc.pose.position, ego_vehicle_location.location,
math.degrees(self._vehicle_yaw))
if magnitude < 60.0 and angle < min(25.0, min_angle):
sel_magnitude = magnitude
sel_traffic_light = traffic_light[0]
min_angle = angle
if sel_traffic_light is not None:
# print('=== Magnitude = {} | Angle = {} | ID = {}'.format(
# sel_magnitude, min_angle, sel_traffic_light))
if self._last_traffic_light is None:
self._last_traffic_light = sel_traffic_light
state = None
for status in self._traffic_lights:
if status.id == sel_traffic_light:
state = status.state
break
if state is None:
self.node.logwarn(
"Couldn't get state of traffic light {}".format(
sel_traffic_light))
return (False, None)
if state == CarlaTrafficLightStatus.RED:
return (True, self._last_traffic_light)
else:
self._last_traffic_light = None
return (False, None)
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 = 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 ros_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))
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 ros_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 = 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
