diff --git a/scripts/myo-rawNode.py b/scripts/myo-rawNode.py index 2836e44..024cef3 100755 --- a/scripts/myo-rawNode.py +++ b/scripts/myo-rawNode.py @@ -78,7 +78,8 @@ def recv_packet(self, timeout=None): while timeout is None or time.time() < t0 + timeout: if timeout is not None: self.ser.timeout = t0 + timeout - time.time() c = self.ser.read() - if not c: return None + if not c: + return None ret = self.proc_byte(ord(c)) if ret: @@ -91,7 +92,8 @@ def recv_packets(self, timeout=.5): t0 = time.time() while time.time() < t0 + timeout: p = self.recv_packet(t0 + timeout - time.time()) - if not p: return res + if not p: + return res res.append(p) return res @@ -167,7 +169,8 @@ def send_command(self, cls, cmd, payload=b'', wait_resp=True): p = self.recv_packet() ## no timeout, so p won't be None - if p.typ == 0: return p + if p.typ == 0: + return p ## not a response: must be an event self.handle_event(p) @@ -190,7 +193,9 @@ def __init__(self, tty=None): self.pose_handlers = [] def detect_tty(self): + print("Detecting tty...") for p in comports(): + print("port p: " + str(p)) if re.search(r'PID=2458:0*1', p[2]): print('using device:', p[0]) return p[0] @@ -371,13 +376,13 @@ def on_arm(self, arm, xdir): connected = 0; print("Initializing...") while(connected == 0): - try: - m = MyoRaw(sys.argv[1] if len(sys.argv) >= 2 else None) - connected = 1; - except (ValueError, KeyboardInterrupt) as e: - print("Myo Armband not found. Attempting to connect...") - rospy.sleep(0.5) - pass + try: + m = MyoRaw(sys.argv[1] if len(sys.argv) >= 2 else None) + connected = 1; + except (ValueError, KeyboardInterrupt) as e: + print("Myo Armband not found. Attempting to connect...") + rospy.sleep(0.5) + pass # Define Publishers imuPub = rospy.Publisher('myo_imu', Imu, queue_size=10) @@ -389,9 +394,9 @@ def on_arm(self, arm, xdir): # Package the EMG data into an EmgArray def proc_emg(emg, moving, times=[]): - ## create an array of ints for emg data - emgPub.publish(emg) - + ## create an array of ints for emg data + emgPub.publish(emg) + ## print framerate of received data times.append(time.time()) if len(times) > 20: @@ -399,31 +404,34 @@ def proc_emg(emg, moving, times=[]): times.pop(0) # Package the IMU data into an Imu message def proc_imu(quat1, acc, gyro): - #for acc values, ~2000 is 1g -> multiply by 0.0048 - #for gyro values....no idea - h=Header() - h.stamp=rospy.Time.now() - h.frame_id='0' - # We currently do not know the covariance of the sensors with each other - cov=[0,0,0,0,0,0,0,0,0] - quat=Quaternion(*quat1) - ## Normalize the quaternion and accelerometer values - quatNorm = math.sqrt(quat.x*quat.x+quat.y*quat.y+quat.z*quat.z+quat.w*quat.w) - normQuat=Quaternion(quat.x/quatNorm, quat.y/quatNorm, quat.z/quatNorm, quat.w/quatNorm) - normAcc=Vector3(acc[0]*0.0048, acc[1]*0.0048, acc[2]*0.0048) - normGyro=Vector3(*gyro) - imu=Imu(h, normQuat, cov, normGyro, cov, normAcc, cov) - imuPub.publish(imu) + # New info: https://github.com/thalmiclabs/myo-bluetooth/blob/master/myohw.h#L292-L295 + # Scale values for unpacking IMU data + # define MYOHW_ORIENTATION_SCALE 16384.0f ///< See myohw_imu_data_t::orientation + # define MYOHW_ACCELEROMETER_SCALE 2048.0f ///< See myohw_imu_data_t::accelerometer + # define MYOHW_GYROSCOPE_SCALE 16.0f ///< See myohw_imu_data_t::gyroscope + h = Header() + h.stamp = rospy.Time.now() + h.frame_id = 'myo' + # We currently do not know the covariance of the sensors with each other + cov = [0, 0, 0, 0, 0, 0, 0, 0, 0] + quat = Quaternion(quat1[0]/16384.0, quat1[1]/16384.0, quat1[2]/16384.0, quat1[3]/16384.0) + ## Normalize the quaternion and accelerometer values + quatNorm = math.sqrt(quat.x*quat.x+quat.y*quat.y+quat.z*quat.z+quat.w*quat.w) + normQuat = Quaternion(quat.x/quatNorm, quat.y/quatNorm, quat.z/quatNorm, quat.w/quatNorm) + normAcc = Vector3(acc[0]/2048.0, acc[1]/2048.0, acc[2]/2048.0) + normGyro = Vector3(gyro[0]/16.0, gyro[1]/16.0, gyro[2]/16.0) + imu = Imu(h, normQuat, cov, normGyro, cov, normAcc, cov) + imuPub.publish(imu) # Package the arm and x-axis direction into an Arm message 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) + #When the arm state changes, publish the new arm and orientation + calibArm=MyoArm(arm.value, xdir.value) + armPub.publish(calibArm) # Publish the value of an enumerated gesture def proc_pose(p): - gestPub.publish(p.value) + gestPub.publish(p.value) m.add_emg_handler(proc_emg) m.add_imu_handler(proc_imu) @@ -440,7 +448,7 @@ def proc_pose(p): except (rospy.ROSInterruptException, serial.serialutil.SerialException) as e: pass finally: - print() - print("Disconnecting...") + print() + print("Disconnecting...") m.disconnect() print()