def displayWaypoints(w_array):
global marker_pub
global map
#delete old markers
for i in range(2,2+displayWaypoints.prevWaypoints):
marker = Marker()
marker.ns = "lab3markers"
marker.id = i
marker.action = Marker.DELETE
marker.header.frame_id = "map"
marker.type = Marker.ARROW
marker_pub.publish(marker)
#publish new markers
for i in range(1,len(w_array)):
marker = Marker()
marker.header.frame_id = "map"
marker.type = Marker.ARROW
marker.header.stamp = rospy.Time.now()
marker.ns = "lab3markers"
marker.id = 2+i-1
marker.color.r = 1
marker.color.a = 1
marker.action = Marker.ADD
marker.scale.x = map.info.resolution/3
marker.scale.y = 2*map.info.resolution/3
marker.points = []
x1 = (w_array[i-1].x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x
y1 = -((w_array[i-1].y+0.5)*map.info.resolution - map.info.origin.position.y)
x2 = (w_array[i].x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x
y2 = -((w_array[i].y+0.5)*map.info.resolution - map.info.origin.position.y)
marker.points.append(Point(x1, y1, 0))
marker.points.append(Point(x2, y2, 0))
marker_pub.publish(marker)
displayWaypoints.prevWaypoints = i
