def _get_current_wp_direction(self, vehicle_position, route):
# for the current position and orientation try to get the closest one from the waypoints
closest_id = 0
closest_waypoint = None
min_distance = 100000
for index in range(len(route)):
waypoint = route[index][0]
computed_distance = distance_vehicle(waypoint, vehicle_position)
if computed_distance < min_distance:
min_distance = computed_distance
closest_id = index
closest_waypoint = waypoint
direction = route[closest_id][1]
if direction == RoadOption.LEFT:
direction = 3.0
elif direction == RoadOption.RIGHT:
direction = 4.0
elif direction == RoadOption.STRAIGHT:
direction = 5.0
else:
direction = 2.0
return closest_waypoint, direction
def get_current_direction(self):
# for the current position and orientation try to get the closest one from the waypoints
vehicle_position = self._ego_actor.get_transform().location
closest_id = 0
min_distance = 100000
for index in range(len(self._route)):
waypoint = self._route[index][0]
computed_distance = distance_vehicle(waypoint, vehicle_position)
if computed_distance < min_distance:
min_distance = computed_distance
closest_id = index
logging.debug("Closest Waypoint %f", closest_id)
direction = self._route[closest_id][1]
return direction