def handle_data(p):
if (p.cls, p.cmd) != (4, 5):
return
c, attr, typ = unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = unpack('8HB', pay)
# not entirely sure what the last byte is,
# but it's a bitmask that seems to indicate which
# sensors think they're being moved around or something
emg = vals[:8]
moving = vals[8]
self.on_emg(emg, moving)
elif attr == 0x1c:
vals = unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.on_imu(quat, acc, gyro)
elif attr == 0x23:
if len(pay) == 6:
try:
typ, val, xdir, _, _, _ = unpack('6B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
elif len(pay) == 3:
try:
typ, val, xdir = unpack('3B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
h = Header()
h.stamp = rospy.Time.now()
if typ == 1: # on arm
self.on_arm(MyoArm(arm=val, xdir=xdir))
elif typ == 2: # removed from arm
self.on_arm(MyoArm(MyoArm.UNKNOWN, MyoArm.UNKNOWN))
elif typ == 3: # pose
if val == 255:
pose = MyoPose(h, 0)
else:
pose = MyoPose(h, val + 1)
self.on_pose(pose)
else:
print('data with unknown attr: %02X %s' % (attr, p))
def proc_arm(arm, xdir):
#When the arm state changes, publish the new arm and orientation
calibArm=MyoArm(arm.value, xdir.value)
armPub.publish(calibArm)
def proc_arm(arm, xdir):
# Edited by Qianli
calibArm = MyoArm(arm, xdir)
armPub.publish(calibArm)