sparse_occupancy_grid_sample.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 from jsk_recognition_msgs.msg import *
5 from geometry_msgs.msg import Point
6 rospy.init_node("test_occupancy_grid")
7 
8 p = rospy.Publisher("/occupancy_grid", SimpleOccupancyGridArray)
9 
10 r = rospy.Rate(1)
11 
12 def cells(x_offset):
13  ret = []
14  for i in range(0, 20):
15  for j in range(0, 20):
16  ret.append(Point(x = 0.05 * i + x_offset, y = 0.05 * j, z = 0))
17  return ret
18 
19 
20 while not rospy.is_shutdown():
21  now = rospy.Time.now()
22  occupancy_grid_array = SimpleOccupancyGridArray()
23  for i in range(10):
24  occupancy_grid = SimpleOccupancyGrid()
25  occupancy_grid.header.frame_id = "map"
26  occupancy_grid.header.stamp = now
27  occupancy_grid.coefficients = [0, 0, 1, i * 0.2]
28  occupancy_grid.resolution = 0.05 #5cm resolution
29  occupancy_grid.cells = cells(i / 2.0)
30  occupancy_grid_array.grids.append(occupancy_grid)
31  occupancy_grid_array.header.stamp = now
32  occupancy_grid_array.header.frame_id = "map"
33  p.publish(occupancy_grid_array)
34  r.sleep()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58