Пример #1
0
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")
Пример #2
0
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")