def og_param_msg(center, size, resolution, occupancy_threshold, frame_id):
og = OccupancyGrid()
og.center.x = center[0, 0]
og.center.y = center[1, 0]
og.center.z = center[2, 0]
og.grid_size.x = size[0, 0]
og.grid_size.y = size[1, 0]
og.grid_size.z = size[2, 0]
og.resolution.x = resolution[0, 0]
og.resolution.y = resolution[1, 0]
og.resolution.z = resolution[2, 0]
og.occupancy_threshold = occupancy_threshold
og.header.frame_id = frame_id
og.header.stamp = rospy.rostime.get_rostime()
return og
def og_param_msg(center, size, resolution, occupancy_threshold, frame_id):
og = OccupancyGrid()
og.center.x = center[0,0]
og.center.y = center[1,0]
og.center.z = center[2,0]
og.grid_size.x = size[0,0]
og.grid_size.y = size[1,0]
og.grid_size.z = size[2,0]
og.resolution.x = resolution[0,0]
og.resolution.y = resolution[1,0]
og.resolution.z = resolution[2,0]
og.occupancy_threshold = occupancy_threshold
og.header.frame_id = frame_id
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
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
