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)
