def og3d_to_og_msg(og3d):
og = OccupancyGrid()
og.center.x = og3d.center[0, 0]
og.center.y = og3d.center[1, 0]
og.center.z = og3d.center[2, 0]
og.grid_size.x = og3d.size[0, 0]
og.grid_size.y = og3d.size[1, 0]
og.grid_size.z = og3d.size[2, 0]
og.resolution.x = og3d.resolution[0, 0]
og.resolution.y = og3d.resolution[1, 0]
og.resolution.z = og3d.resolution[2, 0]
og.occupancy_threshold = 1
og.data = og3d.grid.flatten().tolist()
og.header.frame_id = 'base_link'
og.header.stamp = rospy.rostime.get_rostime()
return og
def og3d_to_og_msg(og3d):
og = OccupancyGrid()
og.center.x = og3d.center[0,0]
og.center.y = og3d.center[1,0]
og.center.z = og3d.center[2,0]
og.grid_size.x = og3d.size[0,0]
og.grid_size.y = og3d.size[1,0]
og.grid_size.z = og3d.size[2,0]
og.resolution.x = og3d.resolution[0,0]
og.resolution.y = og3d.resolution[1,0]
og.resolution.z = og3d.resolution[2,0]
og.occupancy_threshold = 1
og.data = og3d.grid.flatten().tolist()
og.header.frame_id = 'base_link'
og.header.stamp = rospy.rostime.get_rostime()
return og
