def tube_points_lattice_to_tube_edges_sets(tube_points_lattice):
length_scale = tube_points_lattice.length_scale
resolution = tube_points_lattice.resolution
tube_angle_constraint = compute_tube_angle_constraint(length_scale,
resolution)
print "tube angle constraint: " + str(tube_angle_constraint)
tube_edges_sets = tube_edges.TubeEdgesSets(tube_points_lattice,
tube_angle_constraint)
if config.VISUAL_MODE and VISUALIZE_EDGES:
axes_equal = False
land_elevations_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.land_elevations]
plottable_land_elevations = [land_elevations_points, 'b-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(plottable_land_elevations)
lower_tube_envelope_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.lower_tube_envelope]
plottable_lower_tube_envelope = [lower_tube_envelope_points, 'r-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_lower_tube_envelope)
upper_tube_envelope_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.upper_tube_envelope]
plottable_upper_tube_envelope = [upper_tube_envelope_points, 'g-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_upper_tube_envelope)
plottable_edges = tube_edges_sets.get_plottable_edges('k-')
visualize.ELEVATION_PROFILE_PLOT_QUEUE += plottable_edges
visualize.plot_objects(visualize.ELEVATION_PROFILE_PLOT_QUEUE,
axes_equal)
visualize.ELEVATION_PROFILE_PLOT_QUEUE = []
return [tube_edges_sets, resolution]
def elevation_profile_to_tube_points_lattice(elevation_profile,
elevation_mesh_bisection_depth=None,
arc_length_mesh_bisection_depth=None):
if elevation_mesh_bisection_depth == None:
elevation_mesh_bisection_depth = 1
if arc_length_mesh_bisection_depth == None:
arc_length_mesh_bisection_depth = 3
tube_points_lattice = tube_lattice.TubePointsLattice(elevation_profile,
elevation_mesh_bisection_depth, arc_length_mesh_bisection_depth)
if config.VISUAL_MODE and VISUALIZE_LATTICE:
axes_equal = False
land_elevations_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.land_elevations]
plottable_land_elevations = [land_elevations_points, 'b-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(plottable_land_elevations)
lower_tube_envelope_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.lower_tube_envelope]
plottable_lower_tube_envelope = [lower_tube_envelope_points, 'r-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_lower_tube_envelope)
upper_tube_envelope_points = [tube_points_lattice.arc_lengths,
tube_points_lattice.upper_tube_envelope]
plottable_upper_tube_envelope = [upper_tube_envelope_points, 'g-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_upper_tube_envelope)
plottable_lattice = tube_points_lattice.get_plottable_lattice('k.')
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(plottable_lattice)
visualize.plot_objects(visualize.ELEVATION_PROFILE_PLOT_QUEUE,
axes_equal)
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
return tube_points_lattice
def test_path_points(path_points, interpolator, max_curvature, max_error):
sampled_path_points, arc_lengths = sample_path.sample_path_points(
path_points, max_error)
#interpolated_points, curvature_array = interpolator(sampled_path_points,
# resolution)
interpolated_points, curvature_array = interpolator(sampled_path_points,
max_curvature)
#is_curvature_acceptable = curvature.test_curvature_validity(
# curvature_array, max_curvature)
is_error_valid = test_error_validity(sampled_path_points,
interpolated_points, max_error)
if config.VISUAL_MODE and VISUALIZE_CONSTRAINT:
import visualize
path_points_x_vals, path_points_y_vals = zip(*path_points)
plottable_path_points = [path_points_x_vals, path_points_y_vals]
plottable_path = [plottable_path_points, 'r-']
visualize.PLOT_QUEUE_SPATIAL_2D.append(plottable_path)
interpolated_x_vals, interpolated_y_vals = np.transpose(
interpolated_points)
plottable_interpolated_points = [interpolated_x_vals,
interpolated_y_vals]
plottable_interpolation = [plottable_interpolated_points, 'b-']
visualize.PLOT_QUEUE_SPATIAL_2D.append(plottable_interpolation)
visualize.plot_objects(visualize.PLOT_QUEUE_SPATIAL_2D, False)
visualize.PLOT_QUEUE_SPATIAL_2D.pop()
visualize.PLOT_QUEUE_SPATIAL_2D.pop()
return is_error_valid
def paths_3d_to_paths_4d(spatial_paths_sets_3d):
spatiotemporal_paths_sets_4d = \
spatiotemporal_paths_4d.get_spatiotemporal_paths_sets_4d(
spatial_paths_sets_3d, speed_profile_match_landscapes.SpeedProfile)
if config.VISUAL_MODE:
for path_4d in spatiotemporal_paths_sets_4d.selected_paths:
if VISUALIZE_COMFORT:
are_axes_equal = False
plottable_comfort_profile = \
path_4d.get_plottable_comfort_profile('r-')
visualize.COMFORT_PROFILE_PLOT_QUEUE.append(
plottable_comfort_profile)
visualize.plot_objects(visualize.COMFORT_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.COMFORT_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_SPEEDS_BY_ARC_LENGTH:
are_axes_equal = False
plottable_speeds_by_arc_length = \
path_4d.get_plottable_speeds_by_arc_length('g-')
visualize.SPEED_PROFILE_PLOT_QUEUE.append(
plottable_speeds_by_arc_length)
visualize.plot_objects(visualize.SPEED_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.SPEED_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_SPEEDS_BY_TIME:
are_axes_equal = False
plottable_speeds_by_time = \
path_4d.get_plottable_speeds_by_time('r-')
visualize.SPEED_PROFILE_PLOT_QUEUE.append(
plottable_speeds_by_time)
visualize.plot_objects(visualize.SPEED_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.SPEED_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_ACCELS_BY_TIME:
are_axes_equal = False
plottable_accels_by_time = \
path_4d.get_plottable_accels_by_time('b-')
visualize.SPEED_PROFILE_PLOT_QUEUE.append(
plottable_accels_by_time)
visualize.plot_objects(visualize.SPEED_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.SPEED_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_JERK_BY_TIME:
are_axes_equal = False
plottable_jerk_by_time = \
path_4d.get_plottable_jerk_by_time('g-')
visualize.SPEED_PROFILE_PLOT_QUEUE.append(
plottable_jerk_by_time)
visualize.plot_objects(visualize.SPEED_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.SPEED_PROFILE_PLOT_QUEUE.pop()
return spatiotemporal_paths_sets_4d
def city_pair_to_paths_3d(start, end):
result = []
for route_directions in build_directions(start, end):
route_spatial_lattice = build_spatial_lattice(route_directions)
route_spatial_edges_sets = build_spatial_edges_sets(route_spatial_lattice)
route_spatial_graphs_sets = build_spatial_graphs_sets(
route_spatial_edges_sets)
route_spatial_paths_set_2d = build_spatial_paths_set_2d(
route_spatial_graphs_sets)
route_spatial_paths_sets_3d = build_spatial_paths_3d(
route_spatial_paths_set_2d)
if config.VISUAL_MODE:
if len(visualize.PLOT_QUEUE_SPATIAL_2D) > 0:
are_axes_equal = True
visualize.plot_objects(visualize.PLOT_QUEUE_SPATIAL_2D, are_axes_equal)
are_axes_equal)
result.append(route_spatial_paths_sets_3d)
def tube_edges_sets_to_tube_graphs(tube_edges_sets, resolution):
tube_graphs_sets = tube_graphs.TubeGraphsSets(tube_edges_sets,
smoothing_interpolate.bounded_error_graph_interpolation,
resolution)
selected_tube_graphs = tube_graphs_sets.selected_graphs
if config.VISUAL_MODE:
if VISUALIZE_GRAPHS:
print "Num selected tube graphs: " + str(len(selected_tube_graphs))
are_axes_equal = False
land_elevations_points = [tube_edges_sets.arc_lengths,
tube_edges_sets.land_elevations]
plottable_land_elevations = [land_elevations_points, 'b-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_land_elevations)
lower_tube_envelope_points = [tube_edges_sets.arc_lengths,
tube_edges_sets.lower_tube_envelope]
plottable_lower_tube_envelope = [lower_tube_envelope_points, 'r-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_lower_tube_envelope)
upper_tube_envelope_points = [tube_edges_sets.arc_lengths,
tube_edges_sets.upper_tube_envelope]
plottable_upper_tube_envelope = [upper_tube_envelope_points, 'g-']
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_upper_tube_envelope)
plottable_tube_graphs = \
tube_graphs_sets.get_plottable_tube_graphs('k-')
for plottable_tube_graph in plottable_tube_graphs:
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_tube_graph)
visualize.plot_objects(visualize.ELEVATION_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_COST_TIME_SCATTERPLOT:
are_axes_equal = False
cost_time_scatterplot = \
tube_graphs_sets.get_cost_time_scatterplot('r.')
visualize.PLOT_QUEUE_SCATTERPLOT.append(cost_time_scatterplot)
visualize.plot_objects(visualize.PLOT_QUEUE_SCATTERPLOT,
are_axes_equal)
return selected_tube_graphs
def visualize(self):
plottable_tube_profile = self.get_plottable_tube_profile('r-')
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_tube_profile)
plottable_land_profile = self.get_plottable_land_profile('b-')
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_land_profile)
are_elevation_axes_equal = False
visualize.plot_objects(visualize.ELEVATION_PROFILE_PLOT_QUEUE,
are_elevation_axes_equal)
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
plottable_tube_curvature = self.get_plottable_tube_curvature('g-')
visualize.CURVATURE_PROFILE_PLOT_QUEUE.append(
plottable_tube_curvature)
are_curvature_axes_equal = False
visualize.plot_objects(visualize.CURVATURE_PROFILE_PLOT_QUEUE,
are_curvature_axes_equal)
visualize.CURVATURE_PROFILE_PLOT_QUEUE.pop()
plottable_speeds_by_arc_length = \
self.get_plottable_speeds_by_arc_length('c-')
visualize.SPEED_PROFILE_PLOT_QUEUE.append(
plottable_speeds_by_arc_length)
are_speed_axes_equal = False
visualize.plot_objects(visualize.SPEED_PROFILE_PLOT_QUEUE,
are_speed_axes_equal)
visualize.SPEED_PROFILE_PLOT_QUEUE.pop()
def build_spatial_graphs_sets(route_spatial_edges_sets):
"""Build graphs from edges
"""
route_spatial_graphs_sets = spatial_graphs.get_spatial_graphs_sets(
route_spatial_edges_sets)
if config.VISUAL_MODE:
if VISUALIZE_GRAPHS:
plottable_graphs = route_spatial_graphs_sets.get_plottable_graphs('c-')
are_axes_equal = True
for plottable_graph in plottable_graphs:
visualize.PLOT_QUEUE_SPATIAL_2D.append(plottable_graph)
visualize.plot_objects(visualize.PLOT_QUEUE_SPATIAL_2D,
are_axes_equal)
visualize.PLOT_QUEUE_SPATIAL_2D.pop()
if VISUALIZE_GRAPHS_COST_TIME_SCATTERPLOT:
are_axes_equal = False
cost_time_scatterplot = \
route_spatial_graphs_sets.get_cost_time_scatterplot('r.')
visualize.PLOT_QUEUE_SCATTERPLOT.append(cost_time_scatterplot)
visualize.plot_objects(visualize.PLOT_QUEUE_SCATTERPLOT,
are_axes_equal)
return route_spatial_graphs_sets
def build_spatial_paths_set_2d(route_spatial_graphs_sets):
"""Interpolate full length spatial graphs
"""
route_spatial_paths_set_2d = spatial_paths_2d.get_spatial_paths_set_2d(
route_spatial_graphs_sets)
if config.VISUAL_MODE:
if VISUALIZE_PATHS_2D:
plottable_paths_2d = \
route_spatial_paths_set_2d.get_plottable_paths('k-')
plottable_paths_graphs_2d = \
route_spatial_paths_set_2d.get_plottable_graphs('m-')
plottable_paths_and_graphs = zip(plottable_paths_2d,
plottable_paths_graphs_2d)
are_axes_equal = True
for plottable_path_and_graph in plottable_paths_and_graphs:
plottable_path, plottable_graph = plottable_path_and_graph
visualize.PLOT_QUEUE_SPATIAL_2D.append(plottable_path)
visualize.PLOT_QUEUE_SPATIAL_2D.append(plottable_graph)
visualize.plot_objects(visualize.PLOT_QUEUE_SPATIAL_2D,
are_axes_equal)
visualize.PLOT_QUEUE_SPATIAL_2D.pop()
visualize.PLOT_QUEUE_SPATIAL_2D.pop()
return route_spatial_paths_set_2d
def build_spatial_paths_3d(route_spatial_paths_set_2d):
"""Build the best tube elevation options for a given 2d spatial path
"""
route_spatial_paths_sets_3d = spatial_paths_3d.get_spatial_paths_sets_3d(
route_spatial_paths_set_2d)
if config.VISUAL_MODE:
if VISUALIZE_PATHS_3D_ELEVATIONS:
for path_3d in route_spatial_paths_sets_3d.selected_paths:
are_axes_equal = False
plottable_tube_curvature = \
path_3d.get_plottable_tube_curvature('g')
plottable_spatial_curvature = \
path_3d.get_plottable_spatial_curvature('y')
plottable_tube_elevations = \
path_3d.get_plottable_tube_elevations('r')
plottable_land_elevations = \
path_3d.get_plottable_land_elevations('b')
visualize.CURVATURE_PROFILE_PLOT_QUEUE.append(
plottable_tube_curvature)
visualize.CURVATURE_PROFILE_PLOT_QUEUE.append(
plottable_spatial_curvature)
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_tube_elevations)
visualize.ELEVATION_PROFILE_PLOT_QUEUE.append(
plottable_land_elevations)
visualize.plot_objects(visualize.CURVATURE_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.plot_objects(visualize.ELEVATION_PROFILE_PLOT_QUEUE,
are_axes_equal)
visualize.CURVATURE_PROFILE_PLOT_QUEUE.pop()
visualize.CURVATURE_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
visualize.ELEVATION_PROFILE_PLOT_QUEUE.pop()
if VISUALIZE_PATHS_COST_TIME_SCATTERPLOT:
are_axes_equal = False
cost_time_scatterplot = \
route_spatial_paths_sets_3d.get_cost_time_scatterplot('g.')
visualize.PLOT_QUEUE_SCATTERPLOT.append(cost_time_scatterplot)
visualize.plot_objects(visualize.PLOT_QUEUE_SCATTERPLOT,
are_axes_equal)
return route_spatial_paths_sets_3d
