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.test_status != "SUCCESS":
in_radius = math.sqrt(((location.x - self._x)**2) +
((location.y - self._y)**2)) < self._radius
if in_radius:
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"
else:
self.test_status = "RUNNING"
if 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 __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)):
# 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 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))
self.actual_value = self._percentage_route_completed
return new_status
def update(self):
"""
Check if the actor speed is above the speed_threshold
"""
new_status = py_trees.common.Status.RUNNING
linear_speed = CarlaDataProvider.get_velocity(self._actor)
if linear_speed is not None:
if linear_speed < self._speed_threshold and self._time_last_valid_state:
if (GameTime.get_time() - self._time_last_valid_state
) > self._below_threshold_max_time:
# Game over. The actor has been "blocked" for too long
self.test_status = "FAILURE"
# record event
vehicle_location = CarlaDataProvider.get_location(
self._actor)
blocked_event = TrafficEvent(
event_type=TrafficEventType.VEHICLE_BLOCKED)
ActorSpeedAboveThresholdTest._set_event_message(
blocked_event, vehicle_location)
ActorSpeedAboveThresholdTest._set_event_dict(
blocked_event, vehicle_location)
self.list_traffic_events.append(blocked_event)
else:
self._time_last_valid_state = GameTime.get_time()
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 __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._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION)
self.list_traffic_events.append(self._traffic_event)
self._percentage_route_completed = 0.0
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
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)
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
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
