コード例 #1
0
ファイル: myo_node.py プロジェクト: idkm23/myo_raw
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()
コード例 #2
0
ファイル: myo_node.py プロジェクト: ylmeng/grab_box
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()
コード例 #3
0
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"
コード例 #4
0
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"
コード例 #5
0
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()