class MyoNode(object):
"""A ros wrapper for myo_raw"""
def __init__(self):
rospy.init_node('myo_node', anonymous=True)
traning_data_directory = rospy.get_param('~training_data_dir', None)
self.pub_imu = rospy.Publisher('/myo/imu', IMUData, queue_size=10)
self.pub_emg = rospy.Publisher('/myo/emg', EMGData, queue_size=10)
self.pub_pose = rospy.Publisher('/myo/gesture', Gesture, queue_size=10)
self.pub_myo_arm = rospy.Publisher('/myo/arm', MyoArm, queue_size=10, latch=True)
self.pub_ort = rospy.Publisher('/myo/ort', Quaternion, queue_size=10)
os.chdir(rospkg.RosPack().get_path('myo_raw')+'/training_data' if traning_data_directory == None else traning_data_directory)
self.m = Myo(NNClassifier())
def connect(self):
self.m.connect()
self.m.vibrate(2)
self.m.add_emg_handler(self.__on_emg)
self.m.add_imu_handler(self.__on_imu)
# self.m.add_pose_handler(self.__on_pose)
self.m.add_raw_pose_handler(self.__on_raw_pose)
self.m.add_arm_handler(self.__on_arm)
self.baseRot = None
def disconnect(self):
self.m.disconnect()
# builtin pose subscriber
# def __on_pose(self, p):
# self.pub_pose.publish(is_builtin=True, pose_number=p, confidence=0.0)
def __on_arm(self, arm, xdir):
self.pub_myo_arm.publish(arm=arm, xdir=xdir)
def __on_emg(self, emg, moving):
self.pub_emg.publish(emg_data=emg, moving=moving)
def __on_raw_pose(self, p):
self.pub_pose.publish(is_builtin=False, pose_number=p, confidence=0.0)
def __on_imu(self, quat, acc, gyro):
# need to switch the yaw and the roll for some reason
euler = tf.transformations.euler_from_quaternion((quat[0], quat[1], quat[2], quat[3])) # roll, pitch, yaw
self.pub_imu.publish(header=Header(frame_id=rospy.get_param('frame_id', 'map'), stamp=rospy.get_param('stamp', None)),
orientation=Vector3(x=euler[0]*180/pi, y=euler[1]*180/pi, z=euler[2]*180/pi),
angular_velocity=Vector3(x=gyro[0], y=gyro[1], z=gyro[2]),
linear_acceleration=Vector3(x=acc[0]/MYOHW_ACCELEROMETER_SCALE, y=acc[1]/MYOHW_ACCELEROMETER_SCALE, z=acc[2]/MYOHW_ACCELEROMETER_SCALE)
)
if self.baseRot == None:
self.baseRot = euler[0]
# need to switch the yaw and the roll for some reason 2 1 0
rotated_quat = tf.transformations.quaternion_from_euler(-euler[2], euler[1], -euler[0] + self.baseRot)
self.pub_ort.publish(Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3]))
def run(self): # note this function is EXTREAMELY time sensitive... delay will cause a disconnect of the myo
try:
while not rospy.is_shutdown():
self.m.run()
except rospy.ROSInterruptException:
self.m.disconnect()
print "myo node shuttting down"
self.m.disconnect()
class MyoNode(object):
"""A ros wrapper for myo_raw"""
def __init__(self):
rospy.init_node('myo_node', anonymous=True)
traning_data_directory = rospy.get_param('~training_data_dir', None)
self.pub_imu = rospy.Publisher('/myo/imu', PoseStamped, queue_size=10)
self.pub_emg = rospy.Publisher('/myo/emg', EMGData, queue_size=10)
self.pub_pose = rospy.Publisher('/myo/gesture', Gesture, queue_size=10)
self.pub_myo_arm = rospy.Publisher('/myo/arm', MyoArm, queue_size=10, latch=True)
os.chdir(rospkg.RosPack().get_path('myo_raw')+'/training_data' if traning_data_directory == None else traning_data_directory)
self.m = Myo(NNClassifier())
def connect(self):
self.m.connect()
self.m.vibrate(2)
self.m.add_emg_handler(self.__on_emg)
self.m.add_imu_handler(self.__on_imu)
# self.m.add_pose_handler(self.__on_pose)
self.m.add_raw_pose_handler(self.__on_raw_pose)
self.m.add_arm_handler(self.__on_arm)
def disconnect(self):
self.m.disconnect()
# builtin pose subscriber
# def __on_pose(self, p):
# self.pub_pose.publish(is_builtin=True, pose_number=p, confidence=0.0)
def __on_arm(self, arm, xdir):
self.pub_myo_arm.publish(arm=arm, xdir=xdir)
def __on_emg(self, emg, moving):
self.pub_emg.publish(emg_data=emg, moving=moving)
def __on_raw_pose(self, p):
self.pub_pose.publish(is_builtin=False, pose_number=p, confidence=0.0)
def __on_imu(self, quat, acc, gyro):
# need to switch the yaw and the roll for some reason
euler = tf.transformations.euler_from_quaternion((quat[0], quat[1], quat[2], quat[3])) # roll, pitch, yaw
rotated_quat = tf.transformations.quaternion_from_euler(euler[2], euler[1], euler[0])
self.pub_imu.publish(header=Header(frame_id=rospy.get_param('frame_id', 'map')),
pose=Pose(position=Point(x=0,y=0,z=0),
orientation=Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3])))
def run(self): # note this function is EXTREAMELY time sensitive... delay will cause a disconnect of the myo
try:
while not rospy.is_shutdown():
self.m.run()
except rospy.ROSInterruptException:
self.m.disconnect()
print "myo node shuttting down"
self.m.disconnect()
class MyoNode(object):
"""A ros wrapper for myo_raw"""
def __init__(self):
self.m = Myo(NNClassifier())
self.connect(self.m)
if self.m.device_name == LOWER_ARM_DEVICE_NAME:
self.identifier = 'l'
else:
self.identifier = 'u'
#might be necessary for some classification?
#traning_data_directory = rospy.get_param('~training_data_dir', None)
self.pub_imu = rospy.Publisher('/myo/' + self.identifier + '/imu', IMUData, queue_size=10)
self.pub_emg = rospy.Publisher('/myo/' + self.identifier + '/emg', EMGData, queue_size=10)
self.pub_pose = rospy.Publisher('/myo/' + self.identifier + '/gesture', Gesture, queue_size=10)
self.pub_myo_arm = rospy.Publisher('/myo/' + self.identifier + '/arm', MyoArm, queue_size=10, latch=True)
if 'myo-only' in sys.argv:
self.pub_ort = rospy.Publisher('/myo/' + self.identifier + '/ort', Quaternion, queue_size=10)
self.pub_emgimu = rospy.Publisher('/myo/' + self.identifier + '/emgimu', EMGIMU, queue_size=10)
self.downsampler = 0
self.baseRot = None
self.count = 0
self.keepScanning = True
#might be necessary for some classification?
#os.chdir(rospkg.RosPack().get_path('myo_raw')+'/training_data' if traning_data_directory == None else traning_data_directory)
def connect(self, m):
m.connect()
m.vibrate(2)
m.add_emg_handler(self.__on_emg)
m.add_imu_handler(self.__on_imu)
# m.add_pose_handler(self.__on_pose)
m.add_raw_pose_handler(self.__on_raw_pose)
m.add_arm_handler(self.__on_arm)
def disconnect(self):
self.m.disconnect()
# builtin pose subscriber
# def __on_pose(self, p):
# self.pub_pose.publish(is_builtin=True, pose_number=p, confidence=0.0)
def __on_arm(self, arm, xdir):
self.pub_myo_arm.publish(arm=arm, xdir=xdir)
def __on_emg(self, emg, moving):
self.pub_emg.publish(emg_data=emg, moving=moving)
self.emg = emg
def __on_raw_pose(self, p):
self.pub_pose.publish(is_builtin=False, pose_number=p, confidence=0.0)
def __on_imu(self, quat, acc, gyro):
# need to switch the yaw and the roll for some reason
euler = tf.transformations.euler_from_quaternion((quat[0], quat[1], quat[2], quat[3])) # yaw, pitch, roll
if self.baseRot == None:
self.count += 1;
# This subtraction '(60 * pi / 180.0)' is here to create an initial offset which matches a hand's natural initial offset
if self.count > 360:
self.baseRot = euler
return
# need to switch the yaw and the roll for some reason 2 1 0
calibrated = -euler[0] + self.baseRot[0]
if calibrated > pi:
calibrated -= 2*pi
if calibrated < -pi:
calibrated += 2*pi
# exaggerate arm rotation to represent hand motion
exg = -2*(euler[2]-self.baseRot[2]) - self.baseRot[2]
rotated_quat = tf.transformations.quaternion_from_euler(exg, euler[1], calibrated)
if 'myo-only' in sys.argv:
self.pub_ort.publish(Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3]))
self.pub_imu.publish(header=Header(frame_id=rospy.get_param('frame_id', 'map'), stamp=rospy.get_param('stamp', None)),
angular_velocity=Vector3(x=gyro[0], y=gyro[1], z=gyro[2]),
linear_acceleration=Vector3(x=acc[0]/MYOHW_ACCELEROMETER_SCALE, y=acc[1]/MYOHW_ACCELEROMETER_SCALE, z=acc[2]/MYOHW_ACCELEROMETER_SCALE),
orientation=Quaternion(x=rotated_quat[0], y=rotated_quat[1], z=rotated_quat[2], w=rotated_quat[3])
)
if self.emg:
self.pub_emgimu.publish(header=Header(frame_id=rospy.get_param('frame_id', 'map'), stamp=rospy.get_param('stamp', None)),
emg=self.emg,
angular_velocity=gyro,
linear_acceleration=[acc[0]/MYOHW_ACCELEROMETER_SCALE, acc[1]/MYOHW_ACCELEROMETER_SCALE, acc[2]/MYOHW_ACCELEROMETER_SCALE],
orientation=[rotated_quat[0], rotated_quat[1], rotated_quat[2], rotated_quat[3]]
)
def run(self): # note this function is EXTREMELY time sensitive... delay will cause a disconnect of the myo
try:
while not rospy.is_shutdown() and self.keepScanning and not self.m.bt.disconnected:
self.m.run()
except rospy.ROSInterruptException or KeyboardInterrupt:
self.m.disconnect()
print("exiting cause: " + " " + str(not rospy.is_shutdown()) + " " + str(self.keepScanning) + " " + str(not self.m.bt.disconnected))
self.m.initialized = False
print "myo node shutting down"
class MyoNode(object):
"""A ros wrapper for myo_raw"""
def __init__(self):
self.m = Myo(NNClassifier())
self.connect(self.m)
if self.m.device_name == LOWER_ARM_DEVICE_NAME:
self.identifier = 'l'
else:
self.identifier = 'u'
#might be necessary for some classification?
#traning_data_directory = rospy.get_param('~training_data_dir', None)
self.pub_imu = rospy.Publisher('/myo/' + self.identifier + '/imu',
IMUData,
queue_size=10)
self.pub_emg = rospy.Publisher('/myo/' + self.identifier + '/emg',
EMGData,
queue_size=10)
self.pub_pose = rospy.Publisher('/myo/' + self.identifier + '/gesture',
Gesture,
queue_size=10)
self.pub_myo_arm = rospy.Publisher('/myo/' + self.identifier + '/arm',
MyoArm,
queue_size=10,
latch=True)
if 'myo-only' in sys.argv:
self.pub_ort = rospy.Publisher('/myo/' + self.identifier + '/ort',
Quaternion,
queue_size=10)
self.pub_emgimu = rospy.Publisher('/myo/' + self.identifier +
'/emgimu',
EMGIMU,
queue_size=10)
self.downsampler = 0
self.baseRot = None
self.count = 0
self.keepScanning = True
#might be necessary for some classification?
#os.chdir(rospkg.RosPack().get_path('myo_raw')+'/training_data' if traning_data_directory == None else traning_data_directory)
def connect(self, m):
m.connect()
m.vibrate(2)
m.add_emg_handler(self.__on_emg)
m.add_imu_handler(self.__on_imu)
# m.add_pose_handler(self.__on_pose)
m.add_raw_pose_handler(self.__on_raw_pose)
m.add_arm_handler(self.__on_arm)
def disconnect(self):
self.m.disconnect()
# builtin pose subscriber
# def __on_pose(self, p):
# self.pub_pose.publish(is_builtin=True, pose_number=p, confidence=0.0)
def __on_arm(self, arm, xdir):
self.pub_myo_arm.publish(arm=arm, xdir=xdir)
def __on_emg(self, emg, moving):
self.pub_emg.publish(emg_data=emg, moving=moving)
self.emg = emg
def __on_raw_pose(self, p):
self.pub_pose.publish(is_builtin=False, pose_number=p, confidence=0.0)
def __on_imu(self, quat, acc, gyro):
# need to switch the yaw and the roll for some reason
euler = tf.transformations.euler_from_quaternion(
(quat[0], quat[1], quat[2], quat[3])) # yaw, pitch, roll
if self.baseRot == None:
self.count += 1
# This subtraction '(60 * pi / 180.0)' is here to create an initial offset which matches a hand's natural initial offset
if self.count > 360:
self.baseRot = euler
return
# need to switch the yaw and the roll for some reason 2 1 0
calibrated = -euler[0] + self.baseRot[0]
if calibrated > pi:
calibrated -= 2 * pi
if calibrated < -pi:
calibrated += 2 * pi
# exaggerate arm rotation to represent hand motion
exg = -2 * (euler[2] - self.baseRot[2]) - self.baseRot[2]
rotated_quat = tf.transformations.quaternion_from_euler(
exg, euler[1], calibrated)
if 'myo-only' in sys.argv:
self.pub_ort.publish(
Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3]))
self.pub_imu.publish(
header=Header(frame_id=rospy.get_param('frame_id', 'map'),
stamp=rospy.get_param('stamp', None)),
angular_velocity=Vector3(x=gyro[0], y=gyro[1], z=gyro[2]),
linear_acceleration=Vector3(x=acc[0] / MYOHW_ACCELEROMETER_SCALE,
y=acc[1] / MYOHW_ACCELEROMETER_SCALE,
z=acc[2] / MYOHW_ACCELEROMETER_SCALE),
orientation=Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3]))
if self.emg:
self.pub_emgimu.publish(
header=Header(frame_id=rospy.get_param('frame_id', 'map'),
stamp=rospy.get_param('stamp', None)),
emg=self.emg,
angular_velocity=gyro,
linear_acceleration=[
acc[0] / MYOHW_ACCELEROMETER_SCALE,
acc[1] / MYOHW_ACCELEROMETER_SCALE,
acc[2] / MYOHW_ACCELEROMETER_SCALE
],
orientation=[
rotated_quat[0], rotated_quat[1], rotated_quat[2],
rotated_quat[3]
])
def run(
self
): # note this function is EXTREMELY time sensitive... delay will cause a disconnect of the myo
try:
while not rospy.is_shutdown(
) and self.keepScanning and not self.m.bt.disconnected:
self.m.run()
except rospy.ROSInterruptException or KeyboardInterrupt:
self.m.disconnect()
print("exiting cause: " + " " + str(not rospy.is_shutdown()) + " " +
str(self.keepScanning) + " " + str(not self.m.bt.disconnected))
self.m.initialized = False
print "myo node shutting down"
class MyoNode(object):
"""A ros wrapper for myo_raw"""
def __init__(self):
rospy.init_node('myo_node', anonymous=True)
traning_data_directory = rospy.get_param('~training_data_dir', None)
self.pub_imu = rospy.Publisher('/myo/imu', IMUData, queue_size=10)
self.pub_emg = rospy.Publisher('/myo/emg', EMGData, queue_size=10)
self.pub_pose = rospy.Publisher('/myo/gesture', Gesture, queue_size=10)
self.pub_myo_arm = rospy.Publisher('/myo/arm',
MyoArm,
queue_size=10,
latch=True)
self.pub_ort = rospy.Publisher('/myo/ort', Quaternion, queue_size=10)
os.chdir(rospkg.RosPack().get_path('myo_raw') +
'/training_data' if traning_data_directory ==
None else traning_data_directory)
self.m = Myo(NNClassifier())
def connect(self):
self.m.connect()
self.m.vibrate(2)
self.m.add_emg_handler(self.__on_emg)
self.m.add_imu_handler(self.__on_imu)
# self.m.add_pose_handler(self.__on_pose)
self.m.add_raw_pose_handler(self.__on_raw_pose)
self.m.add_arm_handler(self.__on_arm)
self.baseRot = None
def disconnect(self):
self.m.disconnect()
# builtin pose subscriber
# def __on_pose(self, p):
# self.pub_pose.publish(is_builtin=True, pose_number=p, confidence=0.0)
def __on_arm(self, arm, xdir):
self.pub_myo_arm.publish(arm=arm, xdir=xdir)
def __on_emg(self, emg, moving):
self.pub_emg.publish(emg_data=emg, moving=moving)
def __on_raw_pose(self, p):
self.pub_pose.publish(is_builtin=False, pose_number=p, confidence=0.0)
def __on_imu(self, quat, acc, gyro):
# need to switch the yaw and the roll for some reason
euler = tf.transformations.euler_from_quaternion(
(quat[0], quat[1], quat[2], quat[3])) # roll, pitch, yaw
self.pub_imu.publish(
header=Header(frame_id=rospy.get_param('frame_id', 'map'),
stamp=rospy.get_param('stamp', None)),
orientation=Vector3(x=euler[0] * 180 / pi,
y=euler[1] * 180 / pi,
z=euler[2] * 180 / pi),
angular_velocity=Vector3(x=gyro[0], y=gyro[1], z=gyro[2]),
linear_acceleration=Vector3(x=acc[0] / MYOHW_ACCELEROMETER_SCALE,
y=acc[1] / MYOHW_ACCELEROMETER_SCALE,
z=acc[2] / MYOHW_ACCELEROMETER_SCALE))
if self.baseRot == None:
self.baseRot = euler[0]
# need to switch the yaw and the roll for some reason 2 1 0
rotated_quat = tf.transformations.quaternion_from_euler(
-euler[2], euler[1], -euler[0] + self.baseRot)
self.pub_ort.publish(
Quaternion(x=rotated_quat[0],
y=rotated_quat[1],
z=rotated_quat[2],
w=rotated_quat[3]))
def run(
self
): # note this function is EXTREAMELY time sensitive... delay will cause a disconnect of the myo
try:
while not rospy.is_shutdown():
self.m.run()
except rospy.ROSInterruptException:
self.m.disconnect()
print "myo node shuttting down"
self.m.disconnect()