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"
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)