Function interpolate_trajectory

dataset/tools/generate_random_routes.py:254–289

Given some raw keypoints interpolate a full dense trajectory to be used by the user. Args: world: an reference to the CARLA world so we can use the planner waypoints_trajectory: the current coarse trajectory hop_resolution: is the resolution, how dense is the provide

(world_map, waypoints_trajectory, hop_resolution=1.0)

def interpolate_trajectory(world_map, waypoints_trajectory, hop_resolution=1.0):
    """
    Given some raw keypoints interpolate a full dense trajectory to be used by the user.
    Args:
        world: an reference to the CARLA world so we can use the planner
        waypoints_trajectory: the current coarse trajectory
        hop_resolution: is the resolution, how dense is the provided trajectory going to be made
    Return: 
        route: full interpolated route both in GPS coordinates and also in its original form.
    """

    dao = GlobalRoutePlannerDAO(world_map, hop_resolution)
    grp = GlobalRoutePlanner(dao)
    grp.setup()
    # Obtain route plan
    route = []
    is_junction = False
    distance = 0

    try:
        for i in range(len(waypoints_trajectory) - 1):   # Goes until the one before the last.

            waypoint = waypoints_trajectory[i]
            waypoint_next = waypoints_trajectory[i + 1]
            interpolated_trace = grp.trace_route(waypoint, waypoint_next)
            for i, wp_tuple in enumerate(interpolated_trace):
                route.append(wp_tuple[0].transform)
                if i > 0:
                    distance += wp_tuple[0].transform.location.distance(interpolated_trace[i-1][0].transform.location)
                if not is_junction:
                    is_junction = wp_tuple[0].is_junction
                # print (wp_tuple[0].transform.location, wp_tuple[1])
    except:
        return None, distance, is_junction

    return route, distance, is_junction

import subprocess