Ejemplo n.º 1
0
def responder(name):
    global path
    global start
    global goal
    global grid
    global need_next
    last = None
    current = None
    # initalize the publishers
    directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10)
    next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10)

    rospy.init_node('navigator', anonymous=True)

    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():

        #publish on all of my publishing topics
        if not stop:
            if not updated or dirty:
                # start a new path
                path = pth.gen_path(start, goal, grid)
                print path, start, goal
                last = 0
                current = 0
            elif need_next:
                # send the next location in the path
                last = current
                current += 1
                need_next = False
            # keep resending the old directions
            if current == len(path):
                next_dest.publish(doneWith(True, goal[0], goal[1]))
            else:
                print path[current]
                directions.publish(path[last][0], path[last][1], path[current][0], path[current][1])
            
        r.sleep()
    rospy.signal_shutdown("should be dead")
Ejemplo n.º 2
0
def responder(name):
    global path, start, goal, grid, need_next, updated, current, stop
    current += 1
    last = None
    # initalize the publishers
    directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10)
    next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10)

    rospy.init_node('navigator', anonymous=True)

    r = rospy.Rate(3) # 10hz
    while not rospy.is_shutdown():

        #publish on all of my publishing topics
        # print updated
        # print grid
        # if zumy is not None:
            # print "grid zumy: " + str((zumy[0], zumy[1]))
        if grid is not None and updated:
            # start a new path
            path = pth.gen_path(start, goal, grid)
            print 'path'+str(path)
            # print 'path, start, goal from explorer: ' + str((path, start, goal))
            # print path, start, goal
            last = 0
            current = 0
            updated = False
            stop = False
        elif not stop:
            if need_next:
                # send the next location in the path
                # print current
                last = current
                current += 1
                need_next = False
            # keep resending the old directions
            if current >= len(path):
                # print "current"
                # print current
                if current == len(path) and cent is not None:
                    print "turn_directive"
                    # plt.imshow(grid, interpolation='nearest')
                    # plt.pause(.001)
                    # plt.show(block=False)
                    directions.publish(startGoal(-2, -2, cent[0], cent[1]))
                else:
                    print "done"
                    directions.publish(startGoal(-3, -3, 3, 3))
                    next_dest.publish(doneWith(True, goal[0], goal[1]))
            else:
                print "current: " + str(path[current])
                gzum = grid.astype(float)
                for pnt in path[current:]:
                    gzum[pnt] = .75
                gzum[zumy] = .5
                plt.imshow(gzum, interpolation='nearest', origin ='lower')
                plt.pause(.001)
                plt.show(block=False)
                # print 'path[current], current dest'
                # print "path current: " + str(path[current])
                # directions.publish(path[last][0], path[last][1], 3, 3)

                directions.publish(path[last][0], path[last][1], path[current][0], path[current][1])
                # directions.publish(3,3,1,1)

        r.sleep()
    rospy.signal_shutdown("should be dead")