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
