def destroy(self):
"""
destroy all the players and sensors
"""
self.loginfo("Destroying spawned objects...")
try:
# destroy vehicles sensors
for actor_id in self.vehicles_sensors:
destroy_object_request = roscomp.get_service_request(
DestroyObject)
destroy_object_request.id = actor_id
self.call_service(self.destroy_object_service,
destroy_object_request,
timeout=0.5,
spin_until_response_received=True)
self.loginfo(
"Object {} successfully destroyed.".format(actor_id))
self.vehicles_sensors = []
# destroy global sensors
for actor_id in self.global_sensors:
destroy_object_request = roscomp.get_service_request(
DestroyObject)
destroy_object_request.id = actor_id
self.call_service(self.destroy_object_service,
destroy_object_request,
timeout=0.5,
spin_until_response_received=True)
self.loginfo(
"Object {} successfully destroyed.".format(actor_id))
self.global_sensors = []
# destroy player
for player_id in self.players:
destroy_object_request = roscomp.get_service_request(
DestroyObject)
destroy_object_request.id = player_id
self.call_service(self.destroy_object_service,
destroy_object_request,
timeout=0.5,
spin_until_response_received=True)
self.loginfo(
"Object {} successfully destroyed.".format(player_id))
self.players = []
except ServiceException:
self.logwarn(
'Could not call destroy service on objects, the ros bridge is probably already shutdown'
)
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 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 _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 _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_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))
