def node():
global serial
serial = CommsSerial("/dev/ttyUSB0", 115200)
rospy.Subscriber("grin_command", GrinSyncCommand, grin_command_callback)
rospy.Subscriber("servo_command", ServoCommand, servo_command_callback)
pub = rospy.Publisher('grin_status', GrinStatus)
rospy.init_node('grin_bridge')
while not rospy.is_shutdown():
data = serial.send_recv(GRIN.MSRSTATUS,None,152)
unpacked = struct.unpack("IIHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH", data)
extract_servo( unpacked, 0 )
grin = GrinStatus()
grin.rear_left.servo_alpha = extract_servo( unpacked, 3 )
grin.rear_left.servo_beta = extract_servo( unpacked, 2 )
grin.rear_left.servo_charli = extract_servo( unpacked, 1 )
grin.mid_left.servo_alpha = extract_servo( unpacked, 7 )
grin.mid_left.servo_beta = extract_servo( unpacked, 6 )
grin.mid_left.servo_charli = extract_servo( unpacked, 5 )
grin.front_left.servo_alpha = extract_servo( unpacked, 11 )
grin.front_left.servo_beta = extract_servo( unpacked, 10 )
grin.front_left.servo_charli = extract_servo( unpacked, 9 )
grin.front_right.servo_alpha = extract_servo( unpacked, 12 )
grin.front_right.servo_beta = extract_servo( unpacked, 13 )
grin.front_right.servo_charli = extract_servo( unpacked, 14 )
grin.mid_right.servo_alpha = extract_servo( unpacked, 16 )
grin.mid_right.servo_beta = extract_servo( unpacked, 17 )
grin.mid_right.servo_charli = extract_servo( unpacked, 18 )
grin.rear_right.servo_alpha = extract_servo( unpacked, 20 )
grin.rear_right.servo_beta = extract_servo( unpacked, 21 )
grin.rear_right.servo_charli = extract_servo( unpacked, 22 )
#rospy.loginfo(grin)
print 'Sending ms on topic: "grin_status"...'
pub.publish(grin)
rospy.sleep(1.0)
def test_manual():
ser = CommsSerial("/dev/ttyUSB0",115200)
while True:
msg = struct.pack("HHHH", 1400,1400,1000,1000 )
ser.send_recv( 0x0002, msg, 0 )
time.sleep(0.25)
msg = struct.pack("HHHH", 1600,1600,2000,2000 )
ser.send_recv( 0x0002, msg, 0 )
time.sleep(0.25)
def node():
if len(sys.argv) < 2:
usage()
print 'ERROR: Not enough parameters'
exit(1)
port_name = sys.argv[1]
print 'Starting XBee node:'
print 'Subscribing:'
print ' <none>'
print 'Publishing:'
print ' joy_status'
pub_joy_status = rospy.Publisher('joy_status', JoyStatus)
rospy.init_node('hex_xbee')
print 'Initializing serial connection on', port_name
ser = CommsSerial(port_name,115200)
while not rospy.is_shutdown():
message = ser.recv()
msg_fields = struct.unpack('fffff??????????',message[2:])
js = JoyStatus()
js.left_x = msg_fields[0]
js.left_y = msg_fields[1]
js.right_x = msg_fields[2]
js.right_y = msg_fields[3]
js.throttle = msg_fields[4]
js.button_0 = msg_fields[5]
js.button_1 = msg_fields[6]
js.button_2 = msg_fields[7]
js.button_3 = msg_fields[8]
js.button_4 = msg_fields[9]
js.button_5 = msg_fields[10]
js.button_6 = msg_fields[11]
js.button_7 = msg_fields[12]
js.button_8 = msg_fields[13]
js.button_9 = msg_fields[14]
pub_joy_status.publish(js)
#rospy.sleep(0.25)
print 'Message (HEX): ',
for d in message:
print hex(ord(d)),
print ''
def node():
global JOY_TIME_RECV, JOY_STATUS
if len(sys.argv) == 1:
print usage()
exit(1)
elif len(sys.argv) == 2:
# Do nothing, execute normally
pass
elif len(sys.argv) == 3:
if sys.argv[1] != 'test':
print usage()
print 'ERROR: Unrecognized parameters'
exit(1)
if sys.argv[2] == 'neutral':
test_neutral()
elif sys.argv[2] == 'manual':
test_manual()
else:
usage()
print 'ERROR:'
else:
print usage()
print 'ERROR: Unrecognized parameters'
exit(1)
port_name = sys.argv[1]
print 'Starting ArduPilot node:'
print 'Subscribing:'
print ' joy_status'
print 'Publishing:'
print ' <none>'
rospy.Subscriber("joy_status", JoyStatus, joy_status_callback)
rospy.init_node('hex_ardupilot')
print 'Initializing serial connection on', port_name
ser = CommsSerial(port_name,115200)
# Sending serial data to arduino too early will trigger bootloader
# So we sleep to let arduino boot up correctly
rospy.sleep(1)
while not rospy.is_shutdown():
if JOY_STATUS != None:
js = JOY_STATUS
JOY_STATUS = None
raw_left = js.left_y + js.left_x
raw_right = js.left_y - js.left_x
if raw_left > 1.0: raw_left = 1.0
if raw_left < -1.0: raw_left = -1.0
if raw_right > 1.0: raw_right = 1.0
if raw_right < -1.0: raw_right = -1.0
track_left = int(raw_left * 500 + 1500)
track_right = int(raw_right * 500 + 1500)
servo_turrent_yaw = int(js.right_x * 500 + 1500)
print 'js.right_x:', js.right_x, 'sty:', servo_turrent_yaw
print 'track_left:', raw_left
print 'track_right:', raw_right
msg = struct.pack("HHHH",
track_left,
track_right,
servo_turrent_yaw,
1500)
ser.send_recv( 0x0002, msg, 0 )
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 paramters'
exit(1)
port_name = sys.argv[1]
#
# Initialize serial connection
#
print 'Initializing serial connection on', port_name
ser = CommsSerial(port_name, 115200)
#
# Initialize PyGame
#
pygame.init()
screen = pygame.display.set_mode((640,480))
is_up = False
is_down = False
is_left = False
is_right = False
running = True
while(running):
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
is_up = True
if event.key == pygame.K_DOWN:
is_down = True
if event.key == pygame.K_LEFT:
is_left = True
if event.key == pygame.K_RIGHT:
is_right = True
if event.type == pygame.KEYUP:
if event.key == pygame.K_UP:
is_up = False
if event.key == pygame.K_DOWN:
is_down = False
if event.key == pygame.K_LEFT:
is_left = False
if event.key == pygame.K_RIGHT:
is_right = False
print 'is_up: ', is_up
print 'is_down: ', is_down
left_x = 0
left_y = 0
right_x = 0
right_y = 0
throttle = 0
if is_up: left_y -= 1.0
if is_down: left_y += 1.0
if is_left: left_x -= 1.0
if is_right: left_x += 1.0
button_0 = False
button_1 = False
button_2 = False
button_3 = False
button_4 = False
button_5 = False
button_6 = False
button_7 = False
button_8 = False
button_9 = False
print 'type: ', type(button_0)
msb = struct.pack('fffff??????????', left_x, left_y,
right_x, right_y, throttle,
button_0, button_1, button_2,
button_3, button_4, button_5,
button_6, button_7, button_8,
button_9 )
print 'Sending: '
for m in msb:
print hex(ord(m)),
print ""
ser.send( 0x0002, msb )
screen.fill( (0, 0, 0) )
pygame.draw.circle( screen, (255,0,0), (50,50), 10 )
pygame.display.flip()
time.sleep(0.25)
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)
#
# Initialize joystick
#
joy = initialize_pygame_joystick()
if joy == None:
print 'ERROR: No joystick found'
exit(1)
running = True
while(running):
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
left_x = joy.get_axis(0)
left_y = -joy.get_axis(1)
right_x = joy.get_axis(3)
right_y = -joy.get_axis(2)
throttle = joy.get_axis(4)
button_0 = joy.get_button(0)
button_1 = joy.get_button(1)
button_2 = joy.get_button(2)
button_3 = joy.get_button(3)
button_4 = joy.get_button(4)
button_5 = joy.get_button(5)
button_6 = joy.get_button(6)
button_7 = joy.get_button(7)
button_8 = joy.get_button(8)
button_9 = joy.get_button(9)
print 'type: ', type(button_0)
msb = struct.pack('fffff??????????', left_x, left_y,
right_x, right_y, throttle,
button_0, button_1, button_2,
button_3, button_4, button_5,
button_6, button_7, button_8,
button_9 )
print 'Sending: '
for m in msb:
print hex(ord(m)),
print ""
ser.send( 0x0002, msb )
time.sleep(0.25)