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")
# sys.exit()
#==============================================================================
# ar_tags = {}
# ar_tags['ar0'] = 'ar_marker_' + sys.argv[1]
# ar_tags['ar1'] = 'ar_marker_' + sys.argv[2]
# ar_tags['arZ'] = 'ar_marker_' + sys.argv[3]
#==============================================================================
# currentPos = sys.argv[1]
# objs_des = sys.argv[2]
zumyname = sys.argv[1]
# listener = tf.TransformListener()
pub = rospy.Publisher('/'+zumyname+'/object_to_find', pixelxy, queue_size = 10)
subs_done = rospy.Subscriber('/'+zumyname+'/complete', doneWith, callback_subs_done)
# subs_state = rospy.Subscriber('/at_pos',int,callback_reached)
# subs_current_pos = rospy.Subscriber('/'+zumyname+'/state_estimate', Transform, callback_state)
subs_blob = rospy.Subscriber('/'+zumyname+'/model', model, callback_blob_xy)
# subs_blob_y = rospy.Subscriber('/'+zumyname+'/model', model, callback_bloby)
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
objs_des = the_chosen_one
# combiner(blobs_x,blobs_y)
try:
# print "objdes" + str(objs_des[0])
pub.publish(pixelxy(objs_des[0,0], objs_des[0,1]))
# print 'objs_des'
# print objs_des
# controller(currentPos,objs_des)
except Exception as e:
# print 'Insufficient Data: controller, ' + str(e)
pass
