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