import rospy from nav_msgs.msg import OccupancyGrid grid = OccupancyGrid() grid.header.stamp = rospy.Time.now() grid.header.frame_id = "map" grid.info.resolution = 0.1 grid.info.width = 100 grid.info.height = 100 grid.info.origin.position.x = -5.0 grid.info.origin.position.y = -5.0 grid.info.origin.orientation.w = 1.0 rospy.init_node("occupancygrid_publisher") pub = rospy.Publisher("grid_map", OccupancyGrid, queue_size=10) rate = rospy.Rate(10) # publish message every 0.1 seconds while not rospy.is_shutdown(): pub.publish(grid) rate.sleep()
import rospy from nav_msgs.msg import OccupancyGrid rospy.init_node("occupancygrid_subscriber") sub = rospy.Subscriber("grid_map", OccupancyGrid, callback) def callback(msg): # assume we want to get the probability value at cell (10, 20) x = 10 y = 20 width = msg.info.width prob = msg.data[x + y*width] rospy.loginfo("Probability at cell (%d, %d): %d", x, y, prob)This code reads the probability value at cell (10, 20) from an OccupancyGrid message received on the "grid_map" topic. It first calculates the position of the cell in the 1-dimensional data array using its x and y coordinates and the width of the grid. It then logs the probability value to the console.