def get_distance_closest_scenarios(route, list_scenarios,
percentage_completed):
# We take the route starting from the vehicle postion there
percentage_completed = percentage_completed / 100.0
route_cut = route[int(percentage_completed * len(route)):]
print(" PERCENTAGE COMPLETED ", percentage_completed)
print(" LEN ROUTE CUT ", len(route_cut))
# TODO only working for scenarios 3 and 4
triggers_scenario3 = []
triggers_scenario4 = []
for scenario in list_scenarios:
# We get all the scenario 3 and 4 triggers
if type(scenario).__name__ == 'DynamicObjectCrossing':
triggers_scenario3.append(scenario._trigger_location)
elif type(scenario).__name__ == 'VehicleTurningRight' or type(
scenario).__name__ == 'VehicleTurningLeft':
triggers_scenario4.append(scenario._trigger_location)
distance_scenario3 = -1
distance_scenario4 = -1
print("TRIGGERS ")
print(triggers_scenario3)
print("#####")
print(triggers_scenario4)
for trigger in triggers_scenario3:
distance, found = get_distance_along_route(route_cut, trigger)
if found == True and distance_scenario3 == -1 or distance < distance_scenario3:
distance_scenario3 = distance
for trigger in triggers_scenario4:
distance, found = get_distance_along_route(route_cut, trigger)
if found == True and distance_scenario4 == -1 or distance < distance_scenario4:
distance_scenario4 = distance
return distance_scenario3, distance_scenario4
def __init__(self, actor, route, location, distance, name="InTriggerDistanceToLocationAlongRoute"):
"""
Setup class members
"""
super(InTriggerDistanceToLocationAlongRoute, self).__init__(name)
self._map = CarlaDataProvider.get_map()
self._actor = actor
self._location = location
self._route = route
self._distance = distance
self._location_distance, _ = get_distance_along_route(self._route, self._location)
def update(self):
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
if current_location is None:
return new_status
if current_location.distance(self._location) < self._distance + 20:
actor_distance, _ = get_distance_along_route(self._route, current_location)
if (self._location_distance < actor_distance + self._distance and
actor_distance < self._location_distance) or \
self._location_distance < 1.0:
new_status = py_trees.common.Status.SUCCESS
return new_status
