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(): 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 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)