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