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 sample_edge_tube_points(self, resolution):
arc_lengths = [self.start_point.arc_length,
self.end_point.arc_length]
tube_elevations = [self.start_point.tube_elevation,
self.end_point.tube_elevation]
tube_edge_end_points = zip(arc_lengths, tube_elevations)
sampled_tube_points, _ = sample_path.sample_path_points(
tube_edge_end_points, resolution)
self.sampled_arc_lengths, self.sampled_tube_elevations = \
np.transpose(sampled_tube_points)
def sample_directions_geospatials(self, directions_geospatials):
sampled_directions_geospatials, arc_lengths = \
sample_path.sample_path_points(directions_geospatials,
self.SPATIAL_BASE_RESOLUTION)
return sampled_directions_geospatials
