def create_vtk_full_terrain_grid(proto): """Generate VTK polydata for the terrain (height) grid from the local grid response.""" # Parse each local grid response to create numpy arrays for each. for local_grid_found in proto: if local_grid_found.local_grid_type_name == "terrain": vision_tform_local_grid = get_a_tform_b( local_grid_found.local_grid.transforms_snapshot, VISION_FRAME_NAME, local_grid_found.local_grid. frame_name_local_grid_data).to_proto() cell_size = local_grid_found.local_grid.extent.cell_size terrain_pts = get_terrain_grid(local_grid_found) if local_grid_found.local_grid_type_name == "terrain_valid": valid_inds = get_valid_pts(local_grid_found) if local_grid_found.local_grid_type_name == "intensity": color = get_intensity_grid(local_grid_found) # Possibly mask invalid cells (filtering the terrain points by validity). color[valid_inds != 0, 3] = 255 # Offset the local grid's pixels to be in the world frame instead of the local grid frame. terrain_pts = offset_grid_pixels(terrain_pts, vision_tform_local_grid, cell_size) # Create the VTK actors for the local grid. polydata = get_vtk_polydata_from_numpy(terrain_pts) add_numpy_to_vtk_object(data_object=polydata, numpy_array=color, array_name='RGBA') return polydata
def create_vtk_obstacle_grid(proto, robot_state_client): """Generate VTK polydata for the obstacle distance grid from the local grid response.""" for local_grid_found in proto: if local_grid_found.local_grid_type_name == "obstacle_distance": local_grid_proto = local_grid_found cell_size = local_grid_found.local_grid.extent.cell_size # Unpack the data field for the local grid. cells_obstacle_dist = unpack_grid(local_grid_proto).astype(np.float32) # Populate the x,y values with a complete combination of all possible pairs for the dimensions in the grid extent. ys, xs = np.mgrid[0:local_grid_proto.local_grid.extent.num_cells_x, 0:local_grid_proto.local_grid.extent.num_cells_y] # Get the estimated height (z value) of the ground in the vision frame. transforms_snapshot = local_grid_proto.local_grid.transforms_snapshot vision_tform_body = get_a_tform_b(transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME) z_ground_in_vision_frame = compute_ground_height_in_vision_frame( robot_state_client) # Numpy vstack makes it so that each column is (x,y,z) for a single no step grid point. The height values come # from the estimated height of the ground plane as if the robot was standing. cell_count = local_grid_proto.local_grid.extent.num_cells_x * local_grid_proto.local_grid.extent.num_cells_y z = np.ones(cell_count, dtype=np.float32) z *= z_ground_in_vision_frame pts = np.vstack( [np.ravel(xs).astype(np.float32), np.ravel(ys).astype(np.float32), z]).T pts[:, [0, 1]] *= (local_grid_proto.local_grid.extent.cell_size, local_grid_proto.local_grid.extent.cell_size) # Determine the coloration of the obstacle grid. Set the inside of the obstacle as a red hue, the outside of the obstacle # as a blue hue, and the border of an obstacle as a green hue. Note that the inside of an obstacle is determined by a # negative distance value in a grid cell, and the outside of an obstacle is determined by a positive distance value in a # grid cell. The border of an obstacle is considered a distance of [0,.33] meters for a grid cell value. color = np.ones([cell_count, 3], dtype=np.uint8) color[:, 0] = (cells_obstacle_dist <= 0.0) color[:, 1] = np.logical_and(0.0 < cells_obstacle_dist, cells_obstacle_dist < 0.33) color[:, 2] = (cells_obstacle_dist >= 0.33) color *= 255 # Offset the grid points to be in the vision frame instead of the local grid frame. vision_tform_local_grid = get_a_tform_b( transforms_snapshot, VISION_FRAME_NAME, local_grid_proto.local_grid.frame_name_local_grid_data) pts = offset_grid_pixels(pts, vision_tform_local_grid, cell_size) # Create the VTK actors for the local grid. polydata = get_vtk_polydata_from_numpy(pts) add_numpy_to_vtk_object(data_object=polydata, numpy_array=color, array_name='RGBA') return polydata
def create_vtk_no_step_grid(proto, robot_state_client): """Generate VTK polydata for the no step grid from the local grid response.""" for local_grid_found in proto: if local_grid_found.local_grid_type_name == "no_step": local_grid_proto = local_grid_found cell_size = local_grid_found.local_grid.extent.cell_size # Unpack the data field for the local grid. cells_no_step = unpack_grid(local_grid_proto).astype(np.float32) # Populate the x,y values with a complete combination of all possible pairs for the dimensions in the grid extent. ys, xs = np.mgrid[0:local_grid_proto.local_grid.extent.num_cells_x, 0:local_grid_proto.local_grid.extent.num_cells_y] # Get the estimated height (z value) of the ground in the vision frame as if the robot was standing. transforms_snapshot = local_grid_proto.local_grid.transforms_snapshot vision_tform_body = get_a_tform_b(transforms_snapshot, VISION_FRAME_NAME, BODY_FRAME_NAME) z_ground_in_vision_frame = compute_ground_height_in_vision_frame( robot_state_client) # Numpy vstack makes it so that each column is (x,y,z) for a single no step grid point. The height values come # from the estimated height of the ground plane. cell_count = local_grid_proto.local_grid.extent.num_cells_x * local_grid_proto.local_grid.extent.num_cells_y cells_est_height = np.ones(cell_count) * z_ground_in_vision_frame pts = np.vstack([ np.ravel(xs).astype(np.float32), np.ravel(ys).astype(np.float32), cells_est_height ]).T pts[:, [0, 1]] *= (local_grid_proto.local_grid.extent.cell_size, local_grid_proto.local_grid.extent.cell_size) # Determine the coloration based on whether or not the region is steppable. The regions that Spot considers it # cannot safely step are colored red, and the regions that are considered safe to step are colored blue. color = np.zeros([cell_count, 3], dtype=np.uint8) color[:, 0] = (cells_no_step <= 0.0) color[:, 2] = (cells_no_step > 0.0) color *= 255 # Offset the grid points to be in the vision frame instead of the local grid frame. vision_tform_local_grid = get_a_tform_b( transforms_snapshot, VISION_FRAME_NAME, local_grid_proto.local_grid.frame_name_local_grid_data) pts = offset_grid_pixels(pts, vision_tform_local_grid, cell_size) # Create the VTK actors for the local grid. polydata = get_vtk_polydata_from_numpy(pts) add_numpy_to_vtk_object(data_object=polydata, numpy_array=color, array_name='RGBA') return polydata