def responder(zumy_name):
# initalize the publishers
grid = [[0 for _ in range(grid_size)] for b in range(grid_size)]
pub = rospy.Publisher("/" + zumy_name + "/grid_with_zumy", gridZumy, queue_size=10)
r = rospy.Rate(4) # 10hz
counter = 0
grid = None
while not rospy.is_shutdown():
#publish on all of my publishing topics
# pub.publish(griZumy( ))
# print fimage
# print "z"+ str(zumyxy)
# print "h" + str(homography_aquired)
if fimage is not None and zumyxy is not None and homography_aquired:
if counter > 12 or counter == 0:
print "new grid"
grid = objects_on_grid()
counter = 0
zumy = (int(round((zumyxy[0] - length/2.0)/length)), int(round((zumyxy[1] - length/2.0)/length)))
print zumy
counter += 1
# print "zumy: " + str(zumyxy)
# withZumy = grid.copy().astype(float)
# print "hello"
# print zumyxy
# print zumy
# if zumy[0] < grid_size and zumy[1] < grid_size:
# print zumy
# withZumy[zumy[0], zumy[1]] = .5
# withZumy[1,1] = .5
# print withZumy
# ffim = fimage.copy().T
# redobjx = objx/3
# redobjy = objy/3
# for x in range(13):
# for y in range(13):
# ffim[redobjx[x,y],redobjy[x,y]] = 1
# ffim[redobjx[x,y]+1,redobjy[x,y]] = 1
# ffim[redobjx[x,y],redobjy[x,y]+1] = 1
# ffim[redobjx[x,y]+1,redobjy[x,y]+1] = 1
# plt.imshow(ffim, interpolation='nearest', origin='lower')
# plt.pause(.001)
# plt.show(block=False)
gzum = grid.astype(float)
gzum[zumy] = .5
plt.imshow(gzum, interpolation='nearest', origin='lower')
plt.pause(.001)
plt.show(block=False)
pub.publish(gridZumy(pixelxy(zumy[0], zumy[1]),
filteredImage(grid.astype(bool).flatten().tolist(),
grid_size, grid_size, -1.0)))
r.sleep()
rospy.signal_shutdown("should be dead")
def responder():
global last_image_filtered
# initalize the publishers
pub = rospy.Publisher('/filtered_image', filteredImage, queue_size=10)
rospy.init_node('image_processor', anonymous=True)
# subscriber to at least one
rospy.Subscriber("usb_cam/image_raw", Image, callback)
r = rospy.Rate(.5) # 5sec
while not rospy.is_shutdown():
#publish on all of my publishing topics
# pub.publish(pub_string)
if last_image is not None:
print "image found"
height, width, last_image_filtered = img_tt.run_process(last_image, COMPRESSION_FACTOR)
# last_image_filtered = last_image_filtered[::-1]
# skio.imshow(last_image)
# skio.show()
# skio.imshow(last_image_filtered)
# skio.show()
# gzum = grid.astype(float)
# res = last_image_filtered.astype(bool).flatten().reshape(width, height)
# plt.imshow(res, interpolation='nearest', origin='lower')
# plt.pause(.001)
# plt.show(block=False)
pub.publish(filteredImage(last_image_filtered.astype(bool).flatten().tolist(), height, width, COMPRESSION_FACTOR))
else:
print "image not found"
r.sleep()
rospy.signal_shutdown("should be dead")