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)