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