def cmd_test(client_socket, params):
    # Test message: read 10 bytes
    msg = '' \
        + str(unichr(0x02)) \
        + str(unichr(0x00)) \
        + ''
    
    comms_protocol = CommsProtocol()    
    data_to_send = comms_protocol.encode(msg)

    print 'Sending raw: ',
    for d in data_to_send:
        print hex(ord(d)), ' ',
    print ''

    client_socket.send(data_to_send)

    data = client_socket.recv(1024)

    if data: 
        messages = comms_protocol.decode(data)
        for msg in messages:
            print "Received decoded: ",
            for m in msg:
                print hex(ord(m)),
            print ''
def main():

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

    network_host = sys.argv[1]
    network_port = int(sys.argv[2])
    network_address = (network_host,network_port)
    client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client_socket.connect(network_address)
    
    # Test message: read 10 bytes
    msg = '' \
        + str(unichr(0x01)) \
        + str(unichr(0x00)) \
        + ''
    
    comms_protocol = CommsProtocol()
    
    data_to_send = comms_protocol.encode(msg)
    print 'ThreadNetSend: ',
    for d in data_to_send:
        print hex(ord(d)), ' ',
    print ''
    client_socket.send(data_to_send)

    client_socket.settimeout(0.5);

    data = client_socket.recv(1024)
    if data: 
        messages = comms_protocol.decode(data)
        for msg in messages:
            print "Received decoded: ",
            for m in msg:
                print hex(ord(m)),
            print ''
    
            


    client_socket.close()
class TestClient2:
    def __init__(self, network_host, network_port):
        self.network_address = (network_host,network_port)
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.client_socket.connect(self.network_address)
        self.client_socket.settimeout(0.5);

        self.comms_protocol = CommsProtocol()    
        
    def send(self, msg_id, data):
        data_temp = struct.pack("H", msg_id) + data
        data_to_send = self.comms_protocol.encode(data_temp)
        print 'Sending raw: ',
        for d in data_to_send:
            print hex(ord(d)), ' ',
        print ''
        self.client_socket.send(data_to_send)

    def recv(self):
        data = self.client_socket.recv(1024)
        if data:
            messages = self.comms_protocol.decode(data)
            if len(messages) != 0:
                return messages[0]


    def close(self):
        self.client_socket.close()


    def cmd_test(self):
        # Test message: read 10 bytes
        msg = '' + str(unichr(0x02)) + str(unichr(0x00))
    
        self.send(0x0002,'')
        msg = self.recv()

        print "Received decoded: ",
        for m in msg:
            print hex(ord(m)),
        print ''
class TestClient2:
    def __init__(self, network_host, network_port):
        self.network_address = (network_host,network_port)
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.client_socket.connect(self.network_address)
        self.client_socket.settimeout(1.5);

        self.comms_protocol = CommsProtocol()    
        
    def send(self, msg_id, data):
        data_temp = struct.pack("H", msg_id) + data
        data_to_send = self.comms_protocol.encode(data_temp)
        print 'Sending raw: ',
        for d in data_to_send:
            print hex(ord(d)), ' ',
        print ''
        self.client_socket.send(data_to_send)

    def recv(self, msg_id):
        data = self.client_socket.recv(1024)
        if data:
            messages = self.comms_protocol.decode(data)
            for msg in messages:
                if len(msg) >= 2:
                    tmp = msg[0:2]
                    tmp_id = struct.unpack("H",tmp)[0]
                    if tmp_id == msg_id:
                        return msg[2:]
                    


    def close(self):
        self.client_socket.close()


    def cmd_test(self):
        self.send(CM_MSG_TEST,'')
        msg = self.recv(CM_MSG_TEST)

        if msg:
            print "Received decoded: ",
            for m in msg:
                print hex(ord(m)),
            print ''

    def cmd_timings_get_status(self):
        self.send(CM_MSG_TIMINGS_GET_STATUS,'')
        msg = self.recv(CM_MSG_TIMINGS_GET_STATUS)

        if msg != None and len(msg)==28:
            print "Received decoded: "

            for i in range(len(msg)): 
                #print hex(ord(msg[i]))[2:],
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            now_ms, \
                loop_time_fast_us, loop_time_fast_max_us, \
                loop_time_comms_us, loop_time_comms_max_us, \
                log_us, log_max_us \
                = struct.unpack("IIIIIII",msg)

            # integer divisions
            sec_total = now_ms / 1000
            min_total = sec_total / 60
            hours = min_total / 60
            sec = sec_total % 60
            minutes = min_total % 60
            ms = now_ms % 1000
            
            

            print 'now_ms =', now_ms
            print 'uptime = ', hours, 'h', minutes,'min', sec,'sec', ms,'ms'
            print 'loop_time_fast_us =', loop_time_fast_us
            print 'loop_time_fast_max_us =', loop_time_fast_max_us
            print 'loop_time_comms_us =', loop_time_comms_us
            print 'loop_time_comms_max_us =', loop_time_comms_max_us
            print 'log_us', log_us
            print 'log_max_us', log_max_us

        else:
            print "ERROR: Msg len is ", len(msg), "expected 28"

    def cmd_get_page(self, page_nb):
        data = struct.pack("H", page_nb);
        self.send(CM_MSG_LOG_GET_PAGE,data)
        msg = self.recv(CM_MSG_LOG_GET_PAGE)

        if msg != None:
            print "Received decoded: "

            for i in range(len(msg)): 
                #print hex(ord(msg[i]))[2:],
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

    def cmd_log_start(self):
        self.send(CM_MSG_LOG_START,'')

    def cmd_log_stop(self):
        self.send(CM_MSG_LOG_STOP,'')

    def cmd_log_manager_get_status(self):
        self.send(CM_MSG_LOG_GET_STATUS,'')
        msg = self.recv(CM_MSG_LOG_GET_STATUS)

        if msg != None and len(msg)==4:
            print "Received decoded: "

            for i in range(len(msg)): 
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            next_page, errors, flags = struct.unpack("HBB",msg)

            print 'next_page =', next_page
            print 'errors = ', hex(errors)
            print 'flags = ', hex(flags)

        else:
            print "ERROR: Msg len is ", len(msg), "expected 4"
Exemple #5
0
class HexClient:
    def __init__(self, network_host, network_port):
        self.network_address = (network_host,network_port)
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.client_socket.connect(self.network_address)
        self.client_socket.settimeout(1.5);

        self.comms_protocol = CommsProtocol()    
        
    def send(self, msg_id, data):
        data_temp = struct.pack("H", msg_id) + data
        data_to_send = self.comms_protocol.encode(data_temp)
        #print 'Sending raw: ',
        #for d in data_to_send:
        #    print hex(ord(d)), ' ',
        #print ''
        self.client_socket.send(data_to_send)

    def recv(self, msg_id):
        data = self.client_socket.recv(1024)
        if data:
            messages = self.comms_protocol.decode(data)
            for msg in messages:
                if len(msg) >= 2:
                    tmp = msg[0:2]
                    tmp_id = struct.unpack("H",tmp)[0]
                    if tmp_id == msg_id:
                        return msg[2:]
                    
    def close(self):
        self.client_socket.close()


    def cmd_test(self):
        self.send(MSG_TEST,'')
        msg = self.recv(MSG_TEST)
        return msg

    def cmd_maestro_ctrl_get_state(self):
        self.send(MSG_MAESTRO_CTRL_GET_STATE,'')
        msg = self.recv(MSG_MAESTRO_CTRL_GET_STATE)

        if msg != None and len(msg)==110:
            #print "Received decoded: "

            #for i in range(len(msg)): 
            #    if i % 32 == 0 and i != 0:
            #        print ''
            #    print '%0.2X' % ord(msg[i]),
            #print ''

            decoded = struct.unpack('<HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHBB',msg);

            positions = decoded[0:18]
            speeds = decoded[18:36]
            targets = decoded[36:54]
            error = decoded[54]
            are_moving = decoded[55]

            return positions, speeds, \
                targets, error, are_moving

        else:
            print "ERROR: Msg len is ", len(msg), "expected 110"
            return None

    def cmd_joint_get_state(self, joint_nb):
        data = struct.pack("B", joint_nb);
        self.send(MSG_JOINT_GET_STATE,data)
        msg = self.recv(MSG_JOINT_GET_STATE)

        if msg != None and len(msg)==52:
            #print "Received decoded: "
            #for i in range(len(msg)): 
            #    if i % 32 == 0 and i != 0:
            #        print ''
            #    print '%0.2X' % ord(msg[i]),
            #print ''

            decoded = struct.unpack('<8s8sffffBBffhHHf',msg);

            print 'name:', decoded[0]
            print 'leg_name:', decoded[1]
            print 'center:', decoded[2]
            print 'ratio:', decoded[3]
            print 'min:', decoded[4]
            print 'max:', decoded[5]
            print 'invert:', decoded[6]
            print 'error:', decoded[7]
            print 'current_angle:', decoded[8]
            print 'target_angle:', decoded[9]
            print 'offset_us:', decoded[10]
            print 'current_us:', decoded[11]
            print 'target_us:', decoded[12]
            print 'speed_us:', decoded[13]

        else:
            print "ERROR: Msg len is ", len(msg), "expected 52"

    def cmd_leg_set_target(self, target_X, target_Y, target_Z,
                           time_to_target_ms, leg_nb):

        print 'tx', target_X
        print 'ty', target_Y
        print 'tz', target_Z
        print 'ttt', time_to_target_ms
        print 'legnb', leg_nb

        data = struct.pack("ffffB", target_X, target_Y, target_Z,
                           time_to_target_ms, leg_nb);
        self.send(MSG_LEG_SET_TARGET,data)
        msg = self.recv(MSG_LEG_SET_TARGET)

        if msg != None:
            print "Received decoded: "

            for i in range(len(msg)): 
                #print hex(ord(msg[i]))[2:],
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            print 'Message sent ok'

    def cmd_leg_get_state(self, leg_nb):
        data = struct.pack("B", leg_nb);
        self.send(MSG_LEG_GET_STATE,data)
        msg = self.recv(MSG_LEG_GET_STATE)

        if msg != None and len(msg)==114:
            print "Received decoded: "

            for i in range(len(msg)): 
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            decoded = struct.unpack('<8s8sffffffffffffffffff8s8s8sBB',msg);

            print 'name:', decoded[0]
            print 'tripod_name:', decoded[1]
            print 'leg_x:', decoded[2]
            print 'leg_y:', decoded[3]
            print 'leg_z:', decoded[4]
            print 'target_x:', decoded[5]
            print 'target_y:', decoded[6]
            print 'target_z:', decoded[7]
            print 'current_x:', decoded[8]
            print 'current_y:', decoded[9]
            print 'current_z:', decoded[10]
            print 'lenA:', decoded[11]
            print 'lenB:', decoded[12]
            print 'lenC:', decoded[13]

            print 'prepared_angle_A', decoded[14]
            print 'prepared_angle_B', decoded[15]
            print 'prepared_angle_C', decoded[16]

            print 'prepared_target_X', decoded[17]
            print 'prepared_target_Y', decoded[18]
            print 'prepared_target_Z', decoded[19]

            print 'joint_A_name:', decoded[20]
            print 'joint_B_name:', decoded[21]
            print 'joint_C_name:', decoded[22]
            print 'error', decoded[23]
            print 'valid_prepared_solution', decoded[24]

        else:
            print "ERROR: Msg len is ", len(msg), "expected 114"

    def cmd_tripod_get_state(self, tripod_nb):
        data = struct.pack("B", tripod_nb);
        self.send(MSG_TRIPOD_GET_STATE,data)
        msg = self.recv(MSG_TRIPOD_GET_STATE)

        if msg != None and len(msg)==122:
            #print "Received decoded: "
            #for i in range(len(msg)): 
            #    if i % 32 == 0 and i != 0:
            #        print ''
            #    print '%0.2X' % ord(msg[i]),
            #print ''

            decoded = struct.unpack('<8sfffffffffffffffff8s8s8sfffffBB',msg);

            print 'name:', decoded[0]
            print 'target_X', decoded[1]
            print 'target_Y', decoded[2]
            print 'target_Z', decoded[3]
            print 'target_rotation', decoded[4]

            print 'current_X', decoded[5]
            print 'current_Y', decoded[6]
            print 'current_Z', decoded[7]
            print 'current_rotation', decoded[8]

            print 'leg_A_pos_x', decoded[9]
            print 'leg_A_pos_y', decoded[10]
            print 'leg_A_pos_z', decoded[11]

            print 'leg_B_pos_x', decoded[12]
            print 'leg_B_pos_y', decoded[13]
            print 'leg_B_pos_z', decoded[14]

            print 'leg_C_pos_x', decoded[15]
            print 'leg_C_pos_y', decoded[16]
            print 'leg_C_pos_z', decoded[17]

            print 'leg_A_name', decoded[18]
            print 'leg_B_name', decoded[19]
            print 'leg_C_name', decoded[20]
            
            print 'current_dist', math.sqrt(decoded[21])

            print 'prepared_target_X', decoded[22]
            print 'prepared_target_Y', decoded[23]
            print 'prepared_target_Z', decoded[24]
            print 'prepared_target_R_rad', decoded[25]

            print 'valid_prepared_target', decoded[26]
            print 'errors:', decoded[27]


        else:
            print "ERROR: Msg len is ", len(msg), "expected 122"


    def cmd_tripod_set_target(self, target_X, target_Y, target_Z,
                              target_rotation_rad,
                              time_to_target_ms, tripod_nb):

        print 'tx', target_X
        print 'ty', target_Y
        print 'tz', target_Z
        print 'tr', target_rotation_rad
        print 'ttt', time_to_target_ms
        print 'tripodnb', tripod_nb

        data = struct.pack("fffffB", target_X, target_Y, target_Z,
                           target_rotation_rad, 
                           time_to_target_ms, tripod_nb);
        self.send(MSG_TRIPOD_SET_TARGET,data)
        msg = self.recv(MSG_TRIPOD_SET_TARGET)

        if msg != None:
            print "Received decoded: "

            for i in range(len(msg)): 
                #print hex(ord(msg[i]))[2:],
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            print 'Message sent ok'

    def cmd_time_manager_get_state(self):
        self.send(MSG_TIME_MANAGER_GET_STATE,'')
        msg = self.recv(MSG_TIME_MANAGER_GET_STATE)

        if msg != None and len(msg)==93:
            print "Received decoded: "

            for i in range(len(msg)): 
                if i % 32 == 0 and i != 0:
                    print ''
                print '%0.2X' % ord(msg[i]),
            print ''

            decoded = struct.unpack('<IIIIIIIIIIIIIIIIIIIIIIIB',msg);

            print 'periods:    ', decoded[0:10]
            print 'periods_max:', decoded[10:20]
            print 'total_us:    ', decoded[21]
            print 'total_us_max:', decoded[22]
            print 'current_mark_nb:', decoded[23]
            print 'current_mark:', decoded[20]
        else:
            print "ERROR: Msg len is ", len(msg), "expected 93"


    def cmd_gait_controller_move_vd(self, vd_X, vd_Y,
                                    vd_Z, vd_R):
        #print "vd_X", vd_X
        #print "vd_Y", vd_Y
        #print "vd_Z", vd_Z
        #print "vd_R", vd_R

        data = struct.pack("hhhh", vd_X, vd_Y, vd_Z, vd_R);
        self.send(MSG_GAIT_CONTROLLER_MOVE_VD,data)