diff --git a/src/test/map_test.py b/src/test/map_test.py index 266b5ac65..022e24d38 100644 --- a/src/test/map_test.py +++ b/src/test/map_test.py @@ -3,9 +3,11 @@ from nav_msgs.msg import OccupancyGrid import rospy import math +from PIL import Image +from PIL.ImageOps import flip, grayscale, invert topic = "moving_map" -publisher = rospy.Publisher(topic, OccupancyGrid) +publisher = rospy.Publisher(topic, OccupancyGrid, queue_size=1) rospy.init_node("map_test") @@ -13,20 +15,24 @@ t = 0 +image = flip(grayscale(Image.open("../../images/splash.png"))) + while not rospy.is_shutdown(): grid.header.frame_id = "map" grid.header.stamp = rospy.Time.now() grid.info.map_load_time = rospy.Time.now() - grid.info.resolution = 1.0 - grid.info.width = 3 - grid.info.height = 3 + grid.info.resolution = 0.02 + grid.info.width = image.width + grid.info.height = image.height grid.info.origin.position.x = math.cos(t) grid.info.origin.position.y = math.sin(t) grid.info.origin.orientation.w = 1.0 - grid.data = [0, 20, 40, 60, 80, 100, 120, -10, -100] - # Publish the MarkerArray + image = invert(image) + grid.data = [x - 128 for x in image.getdata()] + + # Publish the OccupancyGrid publisher.publish(grid) - rospy.sleep(0.05) - t += 0.1 + rospy.sleep(2) + t += 1