Exemplo n.º 1
0
def node():
    global LEGS_ANGLES

    print 'Starting Maestro node:'
    print 'Subscribing:'
    print '   legs_status'
    print 'Publishing:'
    print '   <none>'

    draw_grid()
    hex = Hex()
    hex.frame.pos = (0,120,0)

    rospy.Subscriber("legs_status", LegsAngles, legs_angles_callback)

    rospy.init_node('hex_3dviz')

    while not rospy.is_shutdown():
        if LEGS_ANGLES != None:
            hex.set_raw( LEGS_ANGLES.RLA, LEGS_ANGLES.RLB, LEGS_ANGLES.RLC, 
                         LEGS_ANGLES.MLA, LEGS_ANGLES.MLB, LEGS_ANGLES.MLC, 
                         LEGS_ANGLES.FLA, LEGS_ANGLES.FLB, LEGS_ANGLES.FLC, 
                         LEGS_ANGLES.FRA, LEGS_ANGLES.FRB, LEGS_ANGLES.FRC, 
                         LEGS_ANGLES.MRA, LEGS_ANGLES.MRB, LEGS_ANGLES.MRC, 
                         LEGS_ANGLES.RRA, LEGS_ANGLES.RRB, LEGS_ANGLES.RRC )
            LEGS_ANGLES = None
            print "rra: ", LEGS_ANGLES.RRA
        rospy.sleep(0.01)
Exemplo n.º 2
0
def main():

    if len(sys.argv) < 2:
        usage()
        print "ERROR: not enough paramters"
        exit(1)

    port_name = sys.argv[1]

    #
    #   Initialize serial connection
    #
    print "Initializing serial connection on", port_name
    ser = CommsSerial(port_name, 115200)

    hex = Hex(color.white, 0.8)
    hex.set_raw(
        1500,
        1500,
        1500,  # leg rear left
        1500,
        1500,
        1500,  # leg middle left
        2000,
        2000,
        2000,  # leg front left
        2000,
        2000,
        2000,  # leg front right
        1500,
        1500,
        1500,  # leg middle right
        1500,
        1500,
        1500,
    )  # leg rear right
    hex.frame.pos = (0, 120, 0)

    draw_grid()

    while True:
        rate(100)
        hex.set_raw(
            1500,
            1500,
            1500,  # leg rear left
            1500,
            1500,
            1500,  # leg middle left
            2000,
            2000,
            2000,  # leg front left
            2000,
            2000,
            2000,  # leg front right
            1500,
            1500,
            1500,  # leg middle right
            1500,
            1500,
            1500,
        )  # leg rear right

    b = time.time()

    print "Sengin msg"
    ser.ser.flushInput()
    ser.send(0x0002, None)
    message = ser.recv_timeout(0.5)

    for m in message:
        print hex(ord(m)), " ",
    print ""
Exemplo n.º 3
0
def main():

    if len(sys.argv) < 2:
        usage()
        print 'ERROR: not enough parameters'
        exit(1)

    network_host = 'localhost'
    network_port = int(sys.argv[1])
    network_address = (network_host,network_port)
    client_socket = socket.socket(socket.AF_INET, 
                                  socket.SOCK_STREAM)
    client_socket.connect(network_address)
    
    signal.signal(signal.SIGINT, signal_handler)
    
    
    net_send_queue = Queue.Queue()
    net_recv_queue = Queue.Queue()
    
    msg = '' \
        + str(unichr(0x02)) \
        + str(unichr(0x00)) \
        + ''
    
    tsend = RequestSender(client_socket,msg,1,CommsProtocol())
    THREADS.append(tsend)
    tsend.start()
    
    trecv = loopers.ThreadNetRecv(client_socket,network_address,
                                  net_recv_queue,CommsProtocol())
    THREADS.append(trecv)
    trecv.start()
    

    draw_grid()
    hex = Hex(visual.color.white, 0.8)
    hex.set_raw( 1500, 1500, 1500,      # leg rear left
                 1500, 1500, 1500,      # leg middle left
                 1500, 1500, 1500,      # leg front left
                 1500, 1500, 1500,      # leg front right
                 1500, 1500, 1500,      # leg middle right
                 1500, 1500, 1500 )     # leg rear right

    while(True):
        visual.rate(100)

        offsets = ( 100, 0, 0,
                      0, 0, 0,
                    100, 0, 0,
                    100, 0, 0,
                      0, 0, 0,
                    100, 0, 0 )

        try:
            data = net_recv_queue.get_nowait()
            if ord(data[0]) == 0x02 and ord(data[1]) == 0x00:
                try:
                    decoded = struct.unpack('<HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHBB',data);
                    positions = list(decoded[1:19])
                    print positions[0]
                    speeds = decoded[19:37]
                    targets = decoded[37:55]
                    error = decoded[55]
                    are_moving = decoded[56]
                    print "positions", positions
                    print "speeds", speeds
                    print "targets", targets
                    #print error
                    #print are_moving

                    for i in range(len(positions)):
                        positions[i] = positions[i] - offsets[i]

                    hex.set_raw( *positions )
                    
                    '''
                    hex.set_raw( 1550, 1550, 1550,      # leg rear left
                                 1550, 1550, 1550,      # leg middle left
                                 1550, 1550, 1550,      # leg front left
                                 1550, 1550, 1550,      # leg front right
                                 1550, 1550, 1550,      # leg middle right
                                 1550, 1550, 1550 )     # leg rear right
                    '''
                except Exception as e:
                    print "ERROR: Error during unpacking data."
                    raise
                    
        except Queue.Empty:
            pass