jsk_rviz_plugins: sparse_occupancy_grid

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 try:
00004   from jsk_pcl_ros.msg import *
00005 except:
00006   import roslib; roslib.load_manifest("jsk_rviz_plugins")
00007   from jsk_pcl_ros.msg import *
00008 import rospy
00009 import random
00010 
00011 rospy.init_node("test_occupancy_grid")
00012 
00013 p = rospy.Publisher("/sparse_occupancy_grid", SparseOccupancyGridArray)
00014 
00015 r = rospy.Rate(1)
00016 
00017 def buildGrid(indices, position, quaternion):
00018   grid = SparseOccupancyGrid()
00019   grid.resolution = 0.1
00020   grid.origin_pose.position.x = position[0]
00021   grid.origin_pose.position.y = position[1]
00022   grid.origin_pose.position.z = position[2]
00023   grid.origin_pose.orientation.x = quaternion[0]
00024   grid.origin_pose.orientation.y = quaternion[1]
00025   grid.origin_pose.orientation.z = quaternion[2]
00026   grid.origin_pose.orientation.w = quaternion[3]
00027   grid.header.stamp = rospy.Time.now()
00028   grid.header.frame_id = "map"
00029   for column_indices in indices:
00030     column = SparseOccupancyGridColumn()
00031     column.column_index = column_indices[0][0]
00032     for (ci, ri) in column_indices:
00033       cell = SparseOccupancyGridCell()
00034       cell.row_index = ri
00035       cell.value = random.random()
00036       column.cells.append(cell)
00037     grid.columns.append(column)
00038   return grid
00039 
00040 while not rospy.is_shutdown():
00041   # buildup occupancy grid
00042   grid1_indices = [[[-1] + [i] for i in range(20)], 
00043                    [[0] + [i] for i in range(20)], 
00044                    [[-2] + [i] for i in range(20)], 
00045                    [[-3] + [i] for i in range(20)], 
00046                    [[-4] + [i] for i in range(20)]]
00047   grid2_indices = [[[-1] + [i] for i in range(20)], 
00048                    [[0] + [i] for i in range(20)], 
00049                    [[1] + [i] for i in range(20)], 
00050                    [[2] + [i] for i in range(20)], 
00051                    [[3] + [i] for i in range(20)], 
00052                    [[4] + [i] for i in range(20)]]
00053   grid1 = buildGrid(grid1_indices, [0, 0, 0], [0.087156, 0, 0, 0.996195])
00054   grid2 = buildGrid(grid2_indices, [0, 0, 1], [-0.087156, 0, 0, 0.996195])
00055   grid_array = SparseOccupancyGridArray()
00056   grid_array.header.stamp = rospy.Time.now()
00057   grid_array.header.frame_id = "map"
00058   grid_array.grids = [grid1, grid2]
00059   p.publish(grid_array)
00060   r.sleep()