def _count_collisions(weak_self, event): """ Callback to update collision count """ self = weak_self() if not self: return actor_type = None if 'static' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_STATIC elif 'vehicle' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_VEHICLE elif 'walker' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_PEDESTRIAN collision_event = TrafficEvent(type=actor_type) collision_event.set_dict({ 'type': event.other_actor.type_id, 'id': event.other_actor.id }) collision_event.set_message( "Agent collided against object with type={} and id={}".format( event.other_actor.type_id, event.other_actor.id)) self.list_traffic_events.append(collision_event) self.actual_value += 1
def _count_collisions(weak_self, event): """ Callback to update collision count """ self = weak_self() if not self: return registered = False actor_type = None if 'static' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_STATIC self.test_status = "FAILURE" elif 'vehicle' in event.other_actor.type_id: for traffic_event in self.list_traffic_events: if traffic_event.get_type() == TrafficEventType.COLLISION_VEHICLE \ and traffic_event.get_dict()['id'] == event.other_actor.id: registered = True actor_type = TrafficEventType.COLLISION_VEHICLE elif 'walker' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_PEDESTRIAN if not registered: collision_event = TrafficEvent(event_type=actor_type) collision_event.set_dict({'type': event.other_actor.type_id, 'id': event.other_actor.id}) collision_event.set_message("Agent collided against object with type={} and id={}".format( event.other_actor.type_id, event.other_actor.id)) self.list_traffic_events.append(collision_event)
def update(self): """ Check if the actor is running a red light """ new_status = py_trees.common.Status.RUNNING location = self._actor.get_location() if location is None: return new_status if not self._target_stop_sign: # scan for stop signs self._target_stop_sign = self._scan_for_stop_sign() else: # we were in the middle of dealing with a stop sign if not self.is_actor_affected_by_stop(self._actor, self._target_stop_sign): # is the vehicle out of the influence of this stop sign now? if not self._stop_completed: # did we stop? self.test_status = "FAILURE" stop_location = self._target_stop_sign.get_transform( ).location running_stop_event = TrafficEvent( event_type=TrafficEventType.STOP_INFRACTION) running_stop_event.set_message( "Agent ran a stop {} at (x={}, y={}, x={})".format( self._target_stop_sign.id, stop_location.x, stop_location.y, stop_location.z)) running_stop_event.set_dict({ 'id': self._target_stop_sign.id, 'x': stop_location.x, 'y': stop_location.y, 'z': stop_location.z }) self.list_traffic_events.append(running_stop_event) # reset state self._target_stop_sign = None self._stop_completed = False if self._target_stop_sign: # we are already dealing with a target stop sign # # did the ego-vehicle stop? current_speed = CarlaDataProvider.get_velocity(self._actor) if current_speed < self.SPEED_THRESHOLD: self._stop_completed = True if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def _lane_change(weak_self, event): """ Callback to update lane invasion count """ self = weak_self() if not self: return # check the lane direction lane_waypoint = self._map.get_waypoint(self._actor.get_location()) current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id if not (self._last_road_id == current_road_id and self._last_lane_id == current_lane_id): next_waypoint = lane_waypoint.next(2.0)[0] vector_wp = np.array([ next_waypoint.transform.location.x - lane_waypoint.transform.location.x, next_waypoint.transform.location.y - lane_waypoint.transform.location.y ]) vector_actor = np.array([ math.cos(math.radians( self._actor.get_transform().rotation.yaw)), math.sin(math.radians( self._actor.get_transform().rotation.yaw)) ]) ang = math.degrees( math.acos( np.clip( np.dot(vector_actor, vector_wp) / (np.linalg.norm(vector_wp)), -1.0, 1.0))) if ang > self.MAX_ALLOWED_ANGLE: self.test_status = "FAILURE" # is there a difference of orientation greater than MAX_ALLOWED_ANGLE deg with respect of the lane # direction? self._infractions += 1 wrong_way_event = TrafficEvent( type=TrafficEventType.WRONG_WAY_INFRACTION) wrong_way_event.set_message( 'Agent invaded a lane in opposite direction: road_id={}, lane_id={}' .format(current_road_id, current_lane_id)) wrong_way_event.set_dict({ 'road_id': current_road_id, 'lane_id': current_lane_id }) self.list_traffic_events.append(wrong_way_event) # remember the current lane and road self._last_lane_id = current_lane_id self._last_road_id = current_road_id
def update(self): """ Check lane invasion count """ new_status = py_trees.common.Status.RUNNING if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE current_transform = self._actor.get_transform() current_location = current_transform.location current_yaw = current_transform.rotation.yaw rot_x = shapely.affinity.rotate(self.positive_shift, angle=current_yaw, origin=shapely.geometry.Point(0, 0)) rot_nx = shapely.affinity.rotate(self.negative_shift, angle=current_yaw, origin=shapely.geometry.Point(0, 0)) sample_point_right = current_location + carla.Location( x=rot_x.coords[1][0], y=rot_x.coords[1][1]) sample_point_left = current_location + carla.Location( x=rot_nx.coords[1][0], y=rot_nx.coords[1][1]) closest_waypoint_right = self._map.get_waypoint( sample_point_right, lane_type=carla.LaneType.Any) closest_waypoint_left = self._map.get_waypoint( sample_point_left, lane_type=carla.LaneType.Any) if closest_waypoint_right and closest_waypoint_left \ and closest_waypoint_right.lane_type != carla.LaneType.Sidewalk \ and closest_waypoint_left.lane_type != carla.LaneType.Sidewalk: # we are not on a sidewalk self._onsidewalk_active = False else: if not self._onsidewalk_active: onsidewalk_event = TrafficEvent( event_type=TrafficEventType.ON_SIDEWALK_INFRACTION) onsidewalk_event.set_message('Agent invaded the sidewalk') onsidewalk_event.set_dict({ 'x': current_location.x, 'y': current_location.y }) self.list_traffic_events.append(onsidewalk_event) self.test_status = "FAILURE" self.actual_value += 1 self._onsidewalk_active = True self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def update(self): """ Check if the actor location is within trigger region """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) if location is None: return new_status if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE elif self.test_status == "RUNNING" or self.test_status == "INIT": # are we too far away from the route waypoints (i.e., off route)? off_route = True shortest_distance = float('inf') for index in range( max(0, self._current_index - self._wsize), min(self._current_index + self._wsize + 1, self._route_length)): # look for the distance to the current waipoint + windows_size ref_waypoint = self._waypoints[index] distance = math.sqrt(((location.x - ref_waypoint.x)**2) + ((location.y - ref_waypoint.y)**2)) if distance < self.DISTANCE_THRESHOLD \ and distance <= shortest_distance \ and index >= self._current_index: shortest_distance = distance self._current_index = index off_route = False if off_route: route_deviation_event = TrafficEvent( event_type=TrafficEventType.ROUTE_DEVIATION) route_deviation_event.set_message( "Agent deviated from the route at (x={}, y={}, z={})". format(location.x, location.y, location.z)) route_deviation_event.set_dict({ 'x': location.x, 'y': location.y, 'z': location.z }) self.list_traffic_events.append(route_deviation_event) self.test_status = "FAILURE" new_status = py_trees.common.Status.FAILURE self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def update(self): """ Check if the actor location is within trigger region """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) if location is None: return new_status if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE elif self.test_status == "RUNNING" or self.test_status == "INIT": # are we too far away from the route waypoints (i.e., off route)? off_route = True for waypoint in self._waypoints: distance = math.sqrt(((location.x - waypoint.x)**2) + ((location.y - waypoint.y)**2)) if distance < self._radius: off_route = False break if off_route: self._counter_off_route += 1 if self._counter_off_route > self._offroad_max: route_deviation_event = TrafficEvent( type=TrafficEventType.ROUTE_DEVIATION) route_deviation_event.set_message( "Agent deviated from the route at (x={}, y={}, z={})". format(location.x, location.y, location.z)) route_deviation_event.set_dict({ 'x': location.x, 'y': location.y, 'z': location.z }) self.list_traffic_events.append(route_deviation_event) self.test_status = "FAILURE" new_status = py_trees.common.Status.FAILURE self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def update(self): """ Check if the actor is running a red light """ new_status = py_trees.common.Status.RUNNING location = self._actor.get_transform().location if location is None: return new_status ego_waypoint = self._map.get_waypoint(location) tail_pt0 = self.rotate_point(carla.Vector3D(-1.0, 0.0, location.z), self._actor.get_transform().rotation.yaw) tail_pt0 = location + carla.Location(tail_pt0) tail_pt1 = self.rotate_point(carla.Vector3D(-4.0, 0.0, location.z), self._actor.get_transform().rotation.yaw) tail_pt1 = location + carla.Location(tail_pt1) for traffic_light, center, area, waypoints in self._list_traffic_lights: if self.debug: z = 2.1 if traffic_light.state == carla.TrafficLightState.Red: color = carla.Color(255, 0, 0) elif traffic_light.state == carla.TrafficLightState.Green: color = carla.Color(0, 255, 0) else: color = carla.Color(255, 255, 255) self._world.debug.draw_point(center + carla.Location(z=z), size=0.2, color=color, life_time=0.01) for pt in area: self._world.debug.draw_point(pt + carla.Location(z=z), size=0.1, color=color, life_time=0.01) for wp in waypoints: text = "{}.{}".format(wp.road_id, wp.lane_id) self._world.debug.draw_string(wp.transform.location, text, draw_shadow=False, color=color, life_time=0.01) # logic center_loc = carla.Location(center) if self._last_red_light_id and self._last_red_light_id == traffic_light.id: continue if center_loc.distance(location) > self.DISTANCE_LIGHT: continue if traffic_light.state != carla.TrafficLightState.Red: continue for wp in waypoints: if ego_waypoint.road_id == wp.road_id and ego_waypoint.lane_id == wp.lane_id: # this light is red and is affecting our lane! # is the vehicle traversing the stop line? if self.is_vehicle_crossing_line((tail_pt0, tail_pt1), (area[0], area[-1])): self.test_status = "FAILURE" location = traffic_light.get_transform().location red_light_event = TrafficEvent( event_type=TrafficEventType. TRAFFIC_LIGHT_INFRACTION) red_light_event.set_message( "Agent ran a red light {} at (x={}, y={}, x={})". format(traffic_light.id, location.x, location.y, location.z)) red_light_event.set_dict({ 'id': traffic_light.id, 'x': location.x, 'y': location.y, 'z': location.z }) self.list_traffic_events.append(red_light_event) self._last_red_light_id = traffic_light.id break if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
class RouteCompletionTest(Criterion): """ Check at which stage of the route is the actor at each tick """ DISTANCE_THRESHOLD = 15.0 # meters WINDOWS_SIZE = 2 def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False): """ """ super(RouteCompletionTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._actor = actor self._route = route self._wsize = self.WINDOWS_SIZE self._current_index = 0 self._route_length = len(self._route) self._waypoints, _ = zip(*self._route) self.target = self._waypoints[-1] self._accum_meters = [] prev_wp = self._waypoints[0] for i, wp in enumerate(self._waypoints): d = wp.distance(prev_wp) if i > 0: accum = self._accum_meters[i - 1] else: accum = 0 self._accum_meters.append(d + accum) prev_wp = wp self._traffic_event = TrafficEvent( event_type=TrafficEventType.ROUTE_COMPLETION) self.list_traffic_events.append(self._traffic_event) self._percentage_route_completed = 0.0 def update(self): """ Check if the actor location is within trigger region """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) if location is None: return new_status if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE elif self.test_status == "RUNNING" or self.test_status == "INIT": for index in range( self._current_index, min(self._current_index + self._wsize + 1, self._route_length)): # look for the distance to the current waipoint + windows_size ref_waypoint = self._waypoints[index] distance = math.sqrt(((location.x - ref_waypoint.x)**2) + ((location.y - ref_waypoint.y)**2)) if distance < self.DISTANCE_THRESHOLD: # good! segment completed! self._current_index = index self._percentage_route_completed = 100.0 * float(self._accum_meters[self._current_index]) \ / float(self._accum_meters[-1]) self._traffic_event.set_dict( {'route_completed': self._percentage_route_completed}) self._traffic_event.set_message( "Agent has completed > {:.2f}% of the route".format( self._percentage_route_completed)) if self._percentage_route_completed > 99.0 and location.distance( self.target) < self.DISTANCE_THRESHOLD: route_completion_event = TrafficEvent( event_type=TrafficEventType.ROUTE_COMPLETED) route_completion_event.set_message( "Destination was successfully reached") self.list_traffic_events.append(route_completion_event) self.test_status = "SUCCESS" elif self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def update(self): """ Check if the actor is running a red light """ new_status = py_trees.common.Status.RUNNING location = self._actor.get_transform().location if location is None: return new_status # were you in affected by a red traffic light and just decided to ignore it? if self._in_red_light: if self._target_traffic_light.state != carla.TrafficLightState.Red: # it is safe now! self._in_red_light = False self._target_traffic_light = None else: # still red tl_t = self._target_traffic_light.get_transform() transformed_tv = tl_t.transform( self._target_traffic_light.trigger_volume.location) distance = carla.Location(transformed_tv).distance(location) s = self.length(self._target_traffic_light.trigger_volume. extent) + self.length( self._actor.bounding_box.extent) if distance > s and self._target_traffic_light.state == carla.TrafficLightState.Red: # you are running a red light self.test_status = "FAILURE" red_light_event = TrafficEvent( type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION) red_light_event.set_message( "Agent ran a red light {} at (x={}, y={}, x={})". format(self._target_traffic_light.id, location.x, location.y, location.z)) red_light_event.set_dict({ 'id': self._target_traffic_light.id, 'x': location.x, 'y': location.y, 'z': location.z }) self.list_traffic_events.append(red_light_event) # state reset self._in_red_light = False self._target_traffic_light = None # scan for red traffic lights for traffic_light in self._list_traffic_lights: if hasattr(traffic_light, 'trigger_volume'): tl_t = traffic_light.get_transform() transformed_tv = tl_t.transform( traffic_light.trigger_volume.location) distance = carla.Location(transformed_tv).distance(location) s = self.length( traffic_light.trigger_volume.extent) + self.length( self._actor.bounding_box.extent) if distance <= s: # this traffic light is affecting the vehicle if traffic_light.state == carla.TrafficLightState.Red: self._target_traffic_light = traffic_light self._in_red_light = True break if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
class RouteCompletionTest(Criterion): """ Check at which stage of the route is the actor at each tick """ def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False): """ """ super(RouteCompletionTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._actor = actor self._route = route self._current_index = 0 self._route_length = len(self._route) self._waypoints, _ = zip(*self._route) self._traffic_event = TrafficEvent( type=TrafficEventType.ROUTE_COMPLETION) self.list_traffic_events.append(self._traffic_event) self._percentage_route_completed = 0.0 def update(self): """ Check if the actor location is within trigger region """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) if location is None: return new_status if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE elif self.test_status == "RUNNING" or self.test_status == "INIT": best_distance = float("inf") best_index = self._current_index for index in range(self._current_index, self._route_length): ref_waypoint = self._waypoints[index] distance = math.sqrt(((location.x - ref_waypoint.x)**2) + ((location.y - ref_waypoint.y)**2)) if distance < best_distance: best_distance = distance best_index = index self._current_index = best_index self._percentage_route_completed = 100.0 * float( self._current_index) / float(self._route_length) self._traffic_event.set_dict( {'route_completed': self._percentage_route_completed}) self._traffic_event.set_message( "Agent has completed > {:.2f}% of the route".format( self._percentage_route_completed)) self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def _count_collisions(weak_self, event): # pylint: disable=too-many-return-statements """ Callback to update collision count """ self = weak_self() if not self: return actor_location = CarlaDataProvider.get_location(self.actor) # Ignore the current one if it is the same id as before if self.last_id == event.other_actor.id: return # Filter to only a specific actor if self.other_actor and self.other_actor.id != event.other_actor.id: return # Filter to only a specific type if self.other_actor_type: if self.other_actor_type == "miscellaneous": if "traffic" not in event.other_actor.type_id \ and "static" not in event.other_actor.type_id: return else: if self.other_actor_type not in event.other_actor.type_id: return # Ignore it if its too close to a previous collision (avoid micro collisions) for collision_location in self.registered_collisions: distance_vector = actor_location - collision_location distance = math.sqrt( math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) if distance <= self.MIN_AREA_OF_COLLISION: return if ('static' in event.other_actor.type_id or 'traffic' in event.other_actor.type_id) \ and 'sidewalk' not in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_STATIC elif 'vehicle' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_VEHICLE elif 'walker' in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_PEDESTRIAN else: return collision_event = TrafficEvent(event_type=actor_type) collision_event.set_dict({ 'type': event.other_actor.type_id, 'id': event.other_actor.id, 'x': actor_location.x, 'y': actor_location.y, 'z': actor_location.z }) collision_event.set_message( "Agent collided against object with type={} and id={} at (x={}, y={}, z={})" .format(event.other_actor.type_id, event.other_actor.id, round(actor_location.x, 3), round(actor_location.y, 3), round(actor_location.z, 3))) self.test_status = "FAILURE" self.actual_value += 1 self.collision_time = GameTime.get_time() self.registered_collisions.append(actor_location) self.list_traffic_events.append(collision_event) # Number 0: static objects -> ignore it if event.other_actor.id != 0: self.last_id = event.other_actor.id
class RouteCompletionTest(Criterion): """ Check at which stage of the route is the actor at each tick Important parameters: - actor: CARLA actor to be used for this test - route: Route to be checked - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ DISTANCE_THRESHOLD = 10.0 # meters WINDOWS_SIZE = 2 def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False): """ """ super(RouteCompletionTest, self).__init__(name, actor, 100, terminate_on_failure=terminate_on_failure) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._actor = actor self._route = route self._map = CarlaDataProvider.get_map() self._wsize = self.WINDOWS_SIZE self._current_index = 0 self._route_length = len(self._route) self._waypoints, _ = zip(*self._route) self.target = self._waypoints[-5] self._accum_meters = [] prev_wp = self._waypoints[0] for i, wp in enumerate(self._waypoints): d = wp.distance(prev_wp) if i > 0: accum = self._accum_meters[i - 1] else: accum = 0 self._accum_meters.append(d + accum) prev_wp = wp self._traffic_event = TrafficEvent( event_type=TrafficEventType.ROUTE_COMPLETION) self.list_traffic_events.append(self._traffic_event) self._percentage_route_completed = 0.0 def update(self): """ Check if the actor location is within trigger region """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) if location is None: return new_status if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE elif self.test_status == "RUNNING" or self.test_status == "INIT": for index in range( self._current_index, min(self._current_index + self._wsize + 1, self._route_length)): # Get the dot product to know if it has passed this location ref_waypoint = self._waypoints[index] wp = self._map.get_waypoint(ref_waypoint) wp_dir = wp.transform.get_forward_vector( ) # Waypoint's forward vector wp_veh = location - ref_waypoint # vector waypoint - vehicle dot_ve_wp = wp_veh.x * wp_dir.x + wp_veh.y * wp_dir.y + wp_veh.z * wp_dir.z if dot_ve_wp > 0: # good! segment completed! self._current_index = index self._percentage_route_completed = 100.0 * float(self._accum_meters[self._current_index]) \ / float(self._accum_meters[-1]) self._traffic_event.set_dict( {'route_completed': self._percentage_route_completed}) self._traffic_event.set_message( "Agent has completed > {:.2f}% of the route".format( self._percentage_route_completed)) if self._percentage_route_completed > 95.0 and location.distance( self.target) < self.DISTANCE_THRESHOLD: route_completion_event = TrafficEvent( event_type=TrafficEventType.ROUTE_COMPLETED) route_completion_event.set_message( "Destination was successfully reached") self.list_traffic_events.append(route_completion_event) self.test_status = "SUCCESS" self._percentage_route_completed = 100 elif self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status def terminate(self, new_status): """ Set test status to failure if not successful and terminate """ self.actual_value = round(self._percentage_route_completed, 2) if self.test_status == "INIT": self.test_status = "FAILURE" super(RouteCompletionTest, self).terminate(new_status)