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)
| 249 | |
| 250 | |
| 251 | def interpolate_trajectory(world_map, waypoints_trajectory, hop_resolution=1.0): |
| 252 | """ |
| 253 | Given some raw keypoints interpolate a full dense trajectory to be used by the user. |
| 254 | Args: |
| 255 | world: an reference to the CARLA world so we can use the planner |
| 256 | waypoints_trajectory: the current coarse trajectory |
| 257 | hop_resolution: is the resolution, how dense is the provided trajectory going to be made |
| 258 | Return: |
| 259 | route: full interpolated route both in GPS coordinates and also in its original form. |
| 260 | """ |
| 261 | |
| 262 | dao = GlobalRoutePlannerDAO(world_map, hop_resolution) |
| 263 | grp = GlobalRoutePlanner(dao) |
| 264 | grp.setup() |
| 265 | # Obtain route plan |
| 266 | route = [] |
| 267 | is_junction = False |
| 268 | distance = 0 |
| 269 | |
| 270 | try: |
| 271 | for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last. |
| 272 | |
| 273 | waypoint = waypoints_trajectory[i] |
| 274 | waypoint_next = waypoints_trajectory[i + 1] |
| 275 | interpolated_trace = grp.trace_route(waypoint, waypoint_next) |
| 276 | for i, wp_tuple in enumerate(interpolated_trace): |
| 277 | route.append(wp_tuple[0].transform) |
| 278 | if i > 0: |
| 279 | distance += wp_tuple[0].transform.location.distance(interpolated_trace[i-1][0].transform.location) |
| 280 | if not is_junction: |
| 281 | is_junction = wp_tuple[0].is_junction |
| 282 | # print (wp_tuple[0].transform.location, wp_tuple[1]) |
| 283 | except: |
| 284 | return None, distance, is_junction |
| 285 | |
| 286 | return route, distance, is_junction |
| 287 | |
| 288 | |
| 289 | def main(): |