def map_update_to_message(self, update):
'''
:type update: OccupancyGridUpdate
'''
update_ros = OccupancyGridUpdateROS()
# Fill in the header
update_ros.header.stamp = rospy.Time.now()
update_ros.header.frame_id = update.header.frame_id
update_ros.x = update.x
update_ros.y = update.y
update_ros.width = update.width
update_ros.height = update.height
update_ros.data = update.data
return update_ros
str((closest_x, closest_y, cost)))
# Repeat again but with updateable occupancy grid
rospy.loginfo("\n\n\n\nRepeating map test but subscribed to updates")
ogm = OccupancyGridManager('/map', subscribe_to_updates=True)
# Give a moment to initialize
rospy.sleep(1.0)
# simulate an update with adding all of it again
ogu = OccupancyGridUpdate()
ogu.header = map_msg.header
ogu.header.stamp = rospy.Time.now()
ogu.x = ogu.y = 0
ogu.width = map_msg.info.width
ogu.height = map_msg.info.height
ogu.data = map_msg.data
update_pub.publish(ogu)
# Wait a moment for the update to arrive
rospy.sleep(1.0)
# Check bounds are correct (e.g. x is x and y is y)
# Note that X is height, and Y is width, both starting
# from the bottom left corner
print("map info: ")
print(ogm._occ_grid_metadata)
wx1, wy1 = ogm.get_world_x_y(ogm.width, ogm.height)
print("costmap coords ogm.height ogm.width: ")
print((ogm.width, ogm.height))
print("world from map coords:")
print((wx1, wy1))
if ogm.is_in_gridmap(ogm.width - 1, ogm.height - 1):
print("ogm.width - 1: {}, ogm.height - 1: {} is in gridmap.".format(
ogm.width - 1, ogm.height - 1))
else:
print(
"ogm.width - 1: {}, ogm.height - 1: {} is NOT in gridmap.".format(
ogm.width - 1, ogm.height - 1))
# simulate an update with adding all of it again
ogu = OccupancyGridUpdate()
ogu.header = occgrid.header
ogu.header.stamp = rospy.Time.now()
ogu.x = ogu.y = 0
ogu.width = occgrid.info.width
ogu.height = occgrid.info.height
ogu.data = occgrid.data
ogm = OccupancyGridManager('/map', subscribe_to_updates=True)
# Give a moment to the update subscriber to initialize
rospy.sleep(1.0)
update_pub.publish(ogu)
# Wait a moment for the update to arrive
rospy.sleep(1.0)
for y in range(2):
for x in range(10):
if ogm.is_in_gridmap(x, y):
cost = ogm.get_cost_from_costmap_x_y(x, y)
print("x: {}, y: {} is in gridmap (cost: {})".format(
x, y, cost))