diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7803344 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*~ +*.pyc +*.dat diff --git a/CMakeLists.txt~ b/CMakeLists.txt~ deleted file mode 100644 index 6b81f42..0000000 --- a/CMakeLists.txt~ +++ /dev/null @@ -1,164 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ros_myo) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - message_generation -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -# Generate messages in the 'msg' folder - add_message_files( - FILES - MyoArm.msg - EmgArray.msg - ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -# Generate added messages and services with any dependencies listed here - generate_messages( - DEPENDENCIES - std_msgs - ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES ros_myo -# CATKIN_DEPENDS roscpp rospy std_msgs message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(ros_myo -# src/${PROJECT_NAME}/ros_myo.cpp -# ) - -## Declare a cpp executable -# add_executable(ros_myo_node src/ros_myo_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(ros_myo_node ros_myo_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(ros_myo_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ros_myo ros_myo_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_myo.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..b2e8e85 --- /dev/null +++ b/LICENSE @@ -0,0 +1,20 @@ +The MIT License (MIT) + +Copyright (c) 2014 Thomas (TJ) Watson + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, +subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/msg/Arm.msg~ b/msg/Arm.msg~ deleted file mode 100644 index 871974e..0000000 --- a/msg/Arm.msg~ +++ /dev/null @@ -1 +0,0 @@ -in64 num diff --git a/msg/EmgArray.msg~ b/msg/EmgArray.msg~ deleted file mode 100644 index 60c4c5d..0000000 --- a/msg/EmgArray.msg~ +++ /dev/null @@ -1,4 +0,0 @@ -#EmgArray message for the Thalmic Myo, which has 8 EMG sensors -#arranged about the arm - -uint16[] data diff --git a/package.xml~ b/package.xml~ deleted file mode 100644 index 6c10650..0000000 --- a/package.xml~ +++ /dev/null @@ -1,59 +0,0 @@ - - - ros_myo - 0.0.0 - The ros_myo package - - - - - tjwatson - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - - - - \ No newline at end of file diff --git a/scripts/analogTurtle.py~ b/scripts/analogTurtle.py~ deleted file mode 100755 index 746d1a5..0000000 --- a/scripts/analogTurtle.py~ +++ /dev/null @@ -1,80 +0,0 @@ -#!/usr/bin/env python - -## Simple myo demo that listens to std_msgs/UInt8 poses published -## to the 'myo_gest' topic - -import rospy -from std_msgs.msg import UInt8, String -from geometry_msgs.msg import Twist, Vector3 -from ros_myo.msg import MyoArm, EmgArray - -########## Data Enums ########### -# MyoArm.arm___________________ # -# UNKNOWN = 0 # -# RIGHT = 1 # -# Left = 2 # -# MyoArm.xdir___________________# -# UNKNOWN = 0 # -# X_TOWARD_WRIST = 1 # -# X_TOWARD_ELBOW = 2 # -# myo_gest UInt8________________# -# REST = 0 # -# FIST = 1 # -# WAVE_IN = 2 # -# WAVE_OUT = 3 # -# FINGERS_SPREAD = 4 # -# THUMB_TO_PINKY = 5 # -# UNKNOWN = 255 # -################################# - - -if __name__ == '__main__': - - global armState - global xDirState - - rospy.init_node('turtlesim_driver', anonymous=True) - - turtlesimPub = rospy.Publisher("directs", String, queue_size=10) - tsPub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10) - - def setArm(data): - global armState - global xDirState - - armState = data.arm - xDirState = data.xdir - - def drive(gest): - - if gest.data == 2 and armState == 1: #WAVE_IN, RIGHT arm - turtlesimPub.publish("go left") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) - elif gest.data == 2 and armState == 2: #WAVE_IN, LEFT arm - turtlesimPub.publish("go right") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) - elif gest.data == 3 and armState == 1: #WAVE_OUT, RIGHT arm - turtlesimPub.publish("go right") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) - elif gest.data == 3 and armState == 2: #WAVE_OUT, LEFT arm - turtlesimPub.publish("go left") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) - - def strength(emgArr1): - emgArr=emgArr1.data - K = 0.001 - ave=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3]+emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/8 - if ave > 100: - tsPub.publish(Twist(Vector3(K*ave,0,0),Vector3(0,0,0))) - rospy.loginfo(K*ave) - else: - tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0))) - - - rospy.Subscriber("myo_arm", MyoArm, setArm) - rospy.Subscriber("myo_gest", UInt8, drive) - rospy.Subscriber("myo_emg", EmgArray, strength) - rospy.loginfo('running!') - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() diff --git a/scripts/common.pyc b/scripts/common.pyc deleted file mode 100644 index a865719..0000000 Binary files a/scripts/common.pyc and /dev/null differ diff --git a/scripts/myo-rawNode.py~ b/scripts/myo-rawNode.py~ deleted file mode 100755 index 017a685..0000000 --- a/scripts/myo-rawNode.py~ +++ /dev/null @@ -1,433 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import enum -import re -import struct -import sys -import threading -import time -import math - -import serial -from serial.tools.list_ports import comports - -from common import * -import rospy -from std_msgs.msg import String, UInt8, Header, MultiArrayLayout, MultiArrayDimension, Float64MultiArray -from geometry_msgs.msg import Quaternion, Vector3 -from sensor_msgs.msg import Imu -from ros_myo.msg import MyoArm, EmgArray - -def multichr(ords): - if sys.version_info[0] >= 3: - return bytes(ords) - else: - return ''.join(map(chr, ords)) - -def multiord(b): - if sys.version_info[0] >= 3: - return list(b) - else: - return map(ord, b) - -class Arm(enum.Enum): - UNKNOWN = 0 - RIGHT = 1 - LEFT = 2 - -class XDirection(enum.Enum): - UNKNOWN = 0 - X_TOWARD_WRIST = 1 - X_TOWARD_ELBOW = 2 - -class Pose(enum.Enum): - REST = 0 - FIST = 1 - WAVE_IN = 2 - WAVE_OUT = 3 - FINGERS_SPREAD = 4 - THUMB_TO_PINKY = 5 - UNKNOWN = 255 - - -class Packet(object): - def __init__(self, ords): - self.typ = ords[0] - self.cls = ords[2] - self.cmd = ords[3] - self.payload = multichr(ords[4:]) - - def __repr__(self): - return 'Packet(%02X, %02X, %02X, [%s])' % \ - (self.typ, self.cls, self.cmd, - ' '.join('%02X' % b for b in multiord(self.payload))) - - -class BT(object): - '''Implements the non-Myo-specific details of the Bluetooth protocol.''' - def __init__(self, tty): - self.ser = serial.Serial(port=tty, baudrate=9600, dsrdtr=1) - self.buf = [] - self.lock = threading.Lock() - self.handlers = [] - - ## internal data-handling methods - def recv_packet(self, timeout=None): - t0 = time.time() - self.ser.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 - - ret = self.proc_byte(ord(c)) - if ret: - if ret.typ == 0x80: - self.handle_event(ret) - return ret - - def recv_packets(self, timeout=.5): - res = [] - t0 = time.time() - while time.time() < t0 + timeout: - p = self.recv_packet(t0 + timeout - time.time()) - if not p: return res - res.append(p) - return res - - def proc_byte(self, c): - if not self.buf: - if c in [0x00, 0x80, 0x08, 0x88]: - self.buf.append(c) - return None - elif len(self.buf) == 1: - self.buf.append(c) - self.packet_len = 4 + (self.buf[0] & 0x07) + self.buf[1] - return None - else: - self.buf.append(c) - - if self.packet_len and len(self.buf) == self.packet_len: - p = Packet(self.buf) - self.buf = [] - return p - return None - - def handle_event(self, p): - for h in self.handlers: - h(p) - - def add_handler(self, h): - self.handlers.append(h) - - def remove_handler(self, h): - try: self.handlers.remove(h) - except ValueError: pass - - def wait_event(self, cls, cmd): - res = [None] - def h(p): - if p.cls == cls and p.cmd == cmd: - res[0] = p - self.add_handler(h) - while res[0] is None: - self.recv_packet() - self.remove_handler(h) - return res[0] - - ## specific BLE commands - def connect(self, addr): - return self.send_command(6, 3, pack('6sBHHHH', multichr(addr), 0, 6, 6, 64, 0)) - - def get_connections(self): - return self.send_command(0, 6) - - def discover(self): - return self.send_command(6, 2, b'\x01') - - def end_scan(self): - return self.send_command(6, 4) - - def disconnect(self, h): - return self.send_command(3, 0, pack('B', h)) - - def read_attr(self, con, attr): - self.send_command(4, 4, pack('BH', con, attr)) - return self.wait_event(4, 5) - - def write_attr(self, con, attr, val): - self.send_command(4, 5, pack('BHB', con, attr, len(val)) + val) - return self.wait_event(4, 1) - - def send_command(self, cls, cmd, payload=b'', wait_resp=True): - s = pack('4B', 0, len(payload), cls, cmd) + payload - self.ser.write(s) - - while True: - p = self.recv_packet() - - ## no timeout, so p won't be None - if p.typ == 0: return p - - ## not a response: must be an event - self.handle_event(p) - - -class MyoRaw(object): - '''Implements the Myo-specific communication protocol.''' - - def __init__(self, tty=None): - if tty is None: - tty = self.detect_tty() - if tty is None: - raise ValueError('Myo dongle not found!') - - self.bt = BT(tty) - self.conn = None - self.emg_handlers = [] - self.imu_handlers = [] - self.arm_handlers = [] - self.pose_handlers = [] - - def detect_tty(self): - for p in comports(): - if re.search(r'PID=2458:0*1', p[2]): - print('using device:', p[0]) - return p[0] - - return None - - def run(self, timeout=None): - self.bt.recv_packet(timeout) - - def connect(self): - ## stop everything from before - self.bt.end_scan() - self.bt.disconnect(0) - self.bt.disconnect(1) - self.bt.disconnect(2) - - ## start scanning - print('scanning...') - self.bt.discover() - while True: - p = self.bt.recv_packet() - print('scan response:', p) - - if p.payload.endswith(b'\x06\x42\x48\x12\x4A\x7F\x2C\x48\x47\xB9\xDE\x04\xA9\x01\x00\x06\xD5'): - addr = list(multiord(p.payload[2:8])) - break - self.bt.end_scan() - - ## connect and wait for status event - conn_pkt = self.bt.connect(addr) - self.conn = multiord(conn_pkt.payload)[-1] - self.bt.wait_event(3, 0) - - ## get firmware version - fw = self.read_attr(0x17) - _, _, _, _, v0, v1, v2, v3 = unpack('BHBBHHHH', fw.payload) - print('firmware version: %d.%d.%d.%d' % (v0, v1, v2, v3)) - - self.old = (v0 == 0) - - if self.old: - ## don't know what these do; Myo Connect sends them, though we get data - ## fine without them - self.write_attr(0x19, b'\x01\x02\x00\x00') - self.write_attr(0x2f, b'\x01\x00') - self.write_attr(0x2c, b'\x01\x00') - self.write_attr(0x32, b'\x01\x00') - self.write_attr(0x35, b'\x01\x00') - - ## enable EMG data - self.write_attr(0x28, b'\x01\x00') - ## enable IMU data - self.write_attr(0x1d, b'\x01\x00') - - ## Sampling rate of the underlying EMG sensor, capped to 1000. If it's - ## less than 1000, emg_hz is correct. If it is greater, the actual - ## framerate starts dropping inversely. Also, if this is much less than - ## 1000, EMG data becomes slower to respond to changes. In conclusion, - ## 1000 is probably a good value. - C = 1000 - emg_hz = 50 - ## strength of low-pass filtering of EMG data - emg_smooth = 100 - - imu_hz = 50 - - ## send sensor parameters, or we don't get any data - self.write_attr(0x19, pack('BBBBHBBBBB', 2, 9, 2, 1, C, emg_smooth, C // emg_hz, imu_hz, 0, 0)) - - else: - name = self.read_attr(0x03) - print('device name: %s' % name.payload) - - ## enable IMU data - self.write_attr(0x1d, b'\x01\x00') - ## enable on/off arm notifications - self.write_attr(0x24, b'\x02\x00') - - # self.write_attr(0x19, b'\x01\x03\x00\x01\x01') - self.start_raw() - - ## add data handlers - 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: - typ, val, xdir = unpack('3B', pay) - - if typ == 1: # on arm - self.on_arm(Arm(val), XDirection(xdir)) - elif typ == 2: # removed from arm - self.on_arm(Arm.UNKNOWN, XDirection.UNKNOWN) - elif typ == 3: # pose - self.on_pose(Pose(val)) - else: - print('data with unknown attr: %02X %s' % (attr, p)) - - self.bt.add_handler(handle_data) - - - def write_attr(self, attr, val): - if self.conn is not None: - self.bt.write_attr(self.conn, attr, val) - - def read_attr(self, attr): - if self.conn is not None: - return self.bt.read_attr(self.conn, attr) - return None - - def disconnect(self): - if self.conn is not None: - self.bt.disconnect(self.conn) - - def start_raw(self): - '''Sending this sequence for v1.0 firmware seems to enable both raw data and - pose notifications. - ''' - - self.write_attr(0x28, b'\x01\x00') - self.write_attr(0x19, b'\x01\x03\x01\x01\x00') - self.write_attr(0x19, b'\x01\x03\x01\x01\x01') - - def vibrate(self, length): - if length in xrange(1, 4): - ## first byte tells it to vibrate; purpose of second byte is unknown - self.write_attr(0x19, pack('3B', 3, 1, length)) - - - def add_emg_handler(self, h): - self.emg_handlers.append(h) - - def add_imu_handler(self, h): - self.imu_handlers.append(h) - - def add_pose_handler(self, h): - self.pose_handlers.append(h) - - def add_arm_handler(self, h): - self.arm_handlers.append(h) - - - def on_emg(self, emg, moving): - for h in self.emg_handlers: - h(emg, moving) - - def on_imu(self, quat, acc, gyro): - for h in self.imu_handlers: - h(quat, acc, gyro) - - def on_pose(self, p): - for h in self.pose_handlers: - h(p) - - def on_arm(self, arm, xdir): - for h in self.arm_handlers: - h(arm, xdir) - -if __name__ == '__main__': - - m = MyoRaw(sys.argv[1] if len(sys.argv) >= 2 else None) - - imuPub = rospy.Publisher('myo_imu', Imu, queue_size=10) - emgPub = rospy.Publisher('myo_emg', EmgArray, queue_size=10) - armPub = rospy.Publisher('myo_arm', MyoArm, queue_size=10) - gestPub = rospy.Publisher('myo_gest', UInt8, queue_size=10) - - rospy.init_node('myo_raw', anonymous=True) - - def proc_emg(emg, moving, times=[]): - ## create an array of ints for emg data - emgPub.publish(emg) - - ## print framerate of received data - times.append(time.time()) - if len(times) > 20: - #print((len(times) - 1) / (times[-1] - times[0])) - times.pop(0) - - 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) - - 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_pose(p): - #Publish a new gesture (enumerated) - gestPub.publish(p.value) - - m.add_emg_handler(proc_emg) - m.add_imu_handler(proc_imu) - m.add_arm_handler(proc_arm) - m.add_pose_handler(proc_pose) - m.connect() - - - try: - - while not rospy.is_shutdown(): - m.run(1) - - except (rospy.ROSInterruptException, serial.serialutil.SerialException) as e: - pass - finally: - m.disconnect() - print() diff --git a/scripts/myo-turtleSim.py~ b/scripts/myo-turtleSim.py~ deleted file mode 100755 index 4e8bff2..0000000 --- a/scripts/myo-turtleSim.py~ +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python - -## Simple myo demo that listens to std_msgs/UInt8 poses published -## to the 'myo_gest' topic - -import rospy -from std_msgs.msg import UInt8, String -from geometry_msgs.msg import Twist, Vector3 -from ros_myo.msg import MyoArm - -########## Data Enums ########### -# MyoArm.arm___________________ # -# UNKNOWN = 0 # -# RIGHT = 1 # -# Left = 2 # -# MyoArm.xdir___________________# -# UNKNOWN = 0 # -# X_TOWARD_WRIST = 1 # -# X_TOWARD_ELBOW = 2 # -# myo_gest UInt8________________# -# REST = 0 # -# FIST = 1 # -# WAVE_IN = 2 # -# WAVE_OUT = 3 # -# FINGERS_SPREAD = 4 # -# THUMB_TO_PINKY = 5 # -# UNKNOWN = 255 # -################################# - - -if __name__ == '__main__': - - global armState - global xDirState - - rospy.init_node('turtlesim_driver', anonymous=True) - - turtlesimPub = rospy.Publisher("directs", String, queue_size=10) - tsPub = rospy.Publisher("cmd_vel", Twist, queue_size=10) - - def setArm(data): - global armState - global xDirState - - armState = data.arm - xDirState = data.xdir - - def drive(gest): - - if gest.data == 1: #FIST - turtlesimPub.publish("go back") - tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0))) - elif gest.data == 2 and armState == 1: #WAVE_IN, RIGHT arm - turtlesimPub.publish("go left") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) - elif gest.data == 2 and armState == 2: #WAVE_IN, LEFT arm - turtlesimPub.publish("go right") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) - elif gest.data == 3 and armState == 1: #WAVE_OUT, RIGHT arm - turtlesimPub.publish("go right") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) - elif gest.data == 3 and armState == 2: #WAVE_OUT, LEFT arm - turtlesimPub.publish("go left") - tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) - elif gest.data == 4: #FINGERS_SPREAD - turtlesimPub.publish("go forward") - tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0))) - - rospy.Subscriber("myo_arm", MyoArm, setArm) - rospy.Subscriber("myo_gest", UInt8, drive) - rospy.loginfo('running!') - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() diff --git a/scripts/myorawnodeerr~ b/scripts/myorawnodeerr~ deleted file mode 100644 index 64efdf1..0000000 --- a/scripts/myorawnodeerr~ +++ /dev/null @@ -1,399 +0,0 @@ -#!/usr/bin/env python - -import enum -import re -import struct -import sys -import threading -import time -import math - -import serial -from serial.tools.list_ports import comports - -from common import * - -import rospy -from std_msgs.msg import String - -def multichr(ords): - if sys.version_info[0] >= 3: - return bytes(ords) - else: - return ''.join(map(chr, ords)) - -def multiord(b): - if sys.version_info[0] >= 3: - return list(b) - else: - return map(ord, b) - -class Arm(enum.Enum): - UNKNOWN = 0 - RIGHT = 1 - LEFT = 2 - -class XDirection(enum.Enum): - UNKNOWN = 0 - X_TOWARD_WRIST = 1 - X_TOWARD_ELBOW = 2 - -class Pose(enum.Enum): - REST = 0 - FIST = 1 - WAVE_IN = 2 - WAVE_OUT = 3 - FINGERS_SPREAD = 4 - THUMB_TO_PINKY = 5 - UNKNOWN = 255 - - -class Packet(object): - def __init__(self, ords): - self.typ = ords[0] - self.cls = ords[2] - self.cmd = ords[3] - self.payload = multichr(ords[4:]) - - def __repr__(self): - return 'Packet(%02X, %02X, %02X, [%s])' % \ - (self.typ, self.cls, self.cmd, - ' '.join('%02X' % b for b in multiord(self.payload))) - - -class BT(object): - '''Implements the non-Myo-specific details of the Bluetooth protocol.''' - def __init__(self, tty): - self.ser = serial.Serial(port=tty, baudrate=9600, dsrdtr=1) - self.buf = [] - self.lock = threading.Lock() - self.handlers = [] - - ## internal data-handling methods - def recv_packet(self, timeout=None): - t0 = time.time() - self.ser.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 - - ret = self.proc_byte(ord(c)) - if ret: - if ret.typ == 0x80: - self.handle_event(ret) - return ret - - def recv_packets(self, timeout=.5): - res = [] - t0 = time.time() - while time.time() < t0 + timeout: - p = self.recv_packet(t0 + timeout - time.time()) - if not p: return res - res.append(p) - return res - - def proc_byte(self, c): - if not self.buf: - if c in [0x00, 0x80, 0x08, 0x88]: - self.buf.append(c) - return None - elif len(self.buf) == 1: - self.buf.append(c) - self.packet_len = 4 + (self.buf[0] & 0x07) + self.buf[1] - return None - else: - self.buf.append(c) - - if self.packet_len and len(self.buf) == self.packet_len: - p = Packet(self.buf) - self.buf = [] - return p - return None - - def handle_event(self, p): - for h in self.handlers: - h(p) - - def add_handler(self, h): - self.handlers.append(h) - - def remove_handler(self, h): - try: self.handlers.remove(h) - except ValueError: pass - - def wait_event(self, cls, cmd): - res = [None] - def h(p): - if p.cls == cls and p.cmd == cmd: - res[0] = p - self.add_handler(h) - while res[0] is None: - self.recv_packet() - self.remove_handler(h) - return res[0] - - ## specific BLE commands - def connect(self, addr): - return self.send_command(6, 3, pack('6sBHHHH', multichr(addr), 0, 6, 6, 64, 0)) - - def get_connections(self): - return self.send_command(0, 6) - - def discover(self): - return self.send_command(6, 2, b'\x01') - - def end_scan(self): - return self.send_command(6, 4) - - def disconnect(self, h): - return self.send_command(3, 0, pack('B', h)) - - def read_attr(self, con, attr): - self.send_command(4, 4, pack('BH', con, attr)) - return self.wait_event(4, 5) - - def write_attr(self, con, attr, val): - self.send_command(4, 5, pack('BHB', con, attr, len(val)) + val) - return self.wait_event(4, 1) - - def send_command(self, cls, cmd, payload=b'', wait_resp=True): - s = pack('4B', 0, len(payload), cls, cmd) + payload - self.ser.write(s) - - while True: - p = self.recv_packet() - - ## no timeout, so p won't be None - if p.typ == 0: return p - - ## not a response: must be an event - self.handle_event(p) - - -class MyoRaw(object): - '''Implements the Myo-specific communication protocol.''' - - def __init__(self, tty=None): - if tty is None: - tty = self.detect_tty() - if tty is None: - raise ValueError('Myo dongle not found!') - - self.bt = BT(tty) - self.conn = None - self.emg_handlers = [] - self.imu_handlers = [] - self.arm_handlers = [] - self.pose_handlers = [] - - def detect_tty(self): - for p in comports(): - if re.search(r'PID=2458:0*1', p[2]): - print('using device:', p[0]) - return p[0] - - return None - - def run(self, timeout=None): - self.bt.recv_packet(timeout) - - def connect(self): - ## stop everything from before - self.bt.end_scan() - self.bt.disconnect(0) - self.bt.disconnect(1) - self.bt.disconnect(2) - - ## start scanning - print('scanning...') - self.bt.discover() - while True: - p = self.bt.recv_packet() - print('scan response:', p) - - if p.payload.endswith(b'\x06\x42\x48\x12\x4A\x7F\x2C\x48\x47\xB9\xDE\x04\xA9\x01\x00\x06\xD5'): - addr = list(multiord(p.payload[2:8])) - break - self.bt.end_scan() - - ## connect and wait for status event - conn_pkt = self.bt.connect(addr) - self.conn = multiord(conn_pkt.payload)[-1] - self.bt.wait_event(3, 0) - - ## get firmware version - fw = self.read_attr(0x17) - _, _, _, _, v0, v1, v2, v3 = unpack('BHBBHHHH', fw.payload) - print('firmware version: %d.%d.%d.%d' % (v0, v1, v2, v3)) - - self.old = (v0 == 0) - - if self.old: - ## don't know what these do; Myo Connect sends them, though we get data - ## fine without them - self.write_attr(0x19, b'\x01\x02\x00\x00') - self.write_attr(0x2f, b'\x01\x00') - self.write_attr(0x2c, b'\x01\x00') - self.write_attr(0x32, b'\x01\x00') - self.write_attr(0x35, b'\x01\x00') - - ## enable EMG data - self.write_attr(0x28, b'\x01\x00') - ## enable IMU data - self.write_attr(0x1d, b'\x01\x00') - - ## Sampling rate of the underlying EMG sensor, capped to 1000. If it's - ## less than 1000, emg_hz is correct. If it is greater, the actual - ## framerate starts dropping inversely. Also, if this is much less than - ## 1000, EMG data becomes slower to respond to changes. In conclusion, - ## 1000 is probably a good value. - C = 1000 - emg_hz = 50 - ## strength of low-pass filtering of EMG data - emg_smooth = 100 - - imu_hz = 50 - - ## send sensor parameters, or we don't get any data - self.write_attr(0x19, pack('BBBBHBBBBB', 2, 9, 2, 1, C, emg_smooth, C // emg_hz, imu_hz, 0, 0)) - - else: - name = self.read_attr(0x03) - print('device name: %s' % name.payload) - - ## enable IMU data - self.write_attr(0x1d, b'\x01\x00') - ## enable on/off arm notifications - self.write_attr(0x24, b'\x02\x00') - - # self.write_attr(0x19, b'\x01\x03\x00\x01\x01') - self.start_raw() - - ## add data handlers - 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: - typ, val, xdir = unpack('3B', pay) - - if typ == 1: # on arm - self.on_arm(Arm(val), XDirection(xdir)) - elif typ == 2: # removed from arm - self.on_arm(Arm.UNKNOWN, XDirection.UNKNOWN) - elif typ == 3: # pose - self.on_pose(Pose(val)) - else: - print('data with unknown attr: %02X %s' % (attr, p)) - - self.bt.add_handler(handle_data) - - - def write_attr(self, attr, val): - if self.conn is not None: - self.bt.write_attr(self.conn, attr, val) - - def read_attr(self, attr): - if self.conn is not None: - return self.bt.read_attr(self.conn, attr) - return None - - def disconnect(self): - if self.conn is not None: - self.bt.disconnect(self.conn) - - def start_raw(self): - '''Sending this sequence for v1.0 firmware seems to enable both raw data and - pose notifications. - ''' - - self.write_attr(0x28, b'\x01\x00') - self.write_attr(0x19, b'\x01\x03\x01\x01\x00') - self.write_attr(0x19, b'\x01\x03\x01\x01\x01') - - def vibrate(self, length): - if length in xrange(1, 4): - ## first byte tells it to vibrate; purpose of second byte is unknown - self.write_attr(0x19, pack('3B', 3, 1, length)) - - - def add_emg_handler(self, h): - self.emg_handlers.append(h) - - def add_imu_handler(self, h): - self.imu_handlers.append(h) - - def add_pose_handler(self, h): - self.pose_handlers.append(h) - - def add_arm_handler(self, h): - self.arm_handlers.append(h) - - - def on_emg(self, emg, moving): - for h in self.emg_handlers: - h(emg, moving) - - def on_imu(self, quat, acc, gyro): - for h in self.imu_handlers: - h(quat, acc, gyro) - - def on_pose(self, p): - for h in self.pose_handlers: - h(p) - - def on_arm(self, arm, xdir): - for h in self.arm_handlers: - h(arm, xdir) - -if __name__ == '__main__': - - global hello_str - hello_str = "unknown" - m = MyoRaw(sys.argv[1] if len(sys.argv) >= 2 else None) - - try: - pub = rospy.Publisher('chatter', String, queue_size=10) - rospy.init_node('talker', anonymous=True) - rate = rospy.Rate(10) # 10hz - - def proc_emg(emg, moving, times=[]): - times.append(time.time()) - if len(times) > 20: - times.pop(0) - -# def proc_arm(arm, xdir): -# hello_str = "arm is %" % arm -# rospy.loginfo("i'm here!") - - m.add_emg_handler(proc_emg) - m.add_arm_handler(lambda arm, xdir: rospy.loginfo("arm is %" % arm)) - m.connect() - - while not rospy.is_shutdown(): - m.run(1) - hello_str = "hello world %s" % rospy.get_time() - rospy.loginfo(hello_str) - pub.publish(hello_str) - rate.sleep() - except rospy.ROSInterruptException: - pass - finally: - m.disconnect()