Added basic implementation of tap listener

CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(graspit_switch_controller)
+
+find_package(catkin REQUIRED)
+
+catkin_package(
+
+)

package.xml

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<package>
+  <name>graspit_switch_controller</name>
+  <version>0.0.0</version>
+  <description>The graspit_switch_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">David Watkins</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/graspit_speech_recognition_commands</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scripts/switch_controller.py

@@ -0,0 +1,121 @@
+#!/usr/bin/python
+
+# open a microphone in pyAudio and listen for taps
+
+import pyaudio
+import struct
+import math
+
+INITIAL_TAP_THRESHOLD = 0.010
+FORMAT = pyaudio.paInt16 
+SHORT_NORMALIZE = (1.0/32768.0)
+CHANNELS = 2
+RATE = 44100  
+INPUT_BLOCK_TIME = 0.05
+INPUT_FRAMES_PER_BLOCK = int(RATE*INPUT_BLOCK_TIME)
+# if we get this many noisy blocks in a row, increase the threshold
+OVERSENSITIVE = 15.0/INPUT_BLOCK_TIME                    
+# if we get this many quiet blocks in a row, decrease the threshold
+UNDERSENSITIVE = 120.0/INPUT_BLOCK_TIME 
+# if the noise was longer than this many blocks, it's not a 'tap'
+MAX_TAP_BLOCKS = 0.15/INPUT_BLOCK_TIME
+
+def get_rms( block ):
+    # RMS amplitude is defined as the square root of the 
+    # mean over time of the square of the amplitude.
+    # so we need to convert this string of bytes into 
+    # a string of 16-bit samples...
+
+    # we will get one short out for each 
+    # two chars in the string.
+    count = len(block)/2
+    format = "%dh"%(count)
+    shorts = struct.unpack( format, block )
+
+    # iterate over the block.
+    sum_squares = 0.0
+    for sample in shorts:
+        # sample is a signed short in +/- 32768. 
+        # normalize it to 1.0
+        n = sample * SHORT_NORMALIZE
+        sum_squares += n*n
+
+    return math.sqrt( sum_squares / count )
+
+class TapTester(object):
+    def __init__(self):
+        self.pa = pyaudio.PyAudio()
+        self.stream = self.open_mic_stream()
+        self.tap_threshold = INITIAL_TAP_THRESHOLD
+        self.noisycount = MAX_TAP_BLOCKS+1 
+        self.quietcount = 0 
+        self.errorcount = 0
+
+    def stop(self):
+        self.stream.close()
+
+    def find_input_device(self):
+        device_index = None            
+        for i in range( self.pa.get_device_count() ):     
+            devinfo = self.pa.get_device_info_by_index(i)   
+            print( "Device %d: %s"%(i,devinfo["name"]) )
+
+            for keyword in ["mic","input"]:
+                if keyword in devinfo["name"].lower():
+                    print( "Found an input: device %d - %s"%(i,devinfo["name"]) )
+                    device_index = i
+                    return device_index
+
+        if device_index == None:
+            print( "No preferred input found; using default input device." )
+
+        return device_index
+
+    def open_mic_stream( self ):
+        # device_index = self.find_input_device()
+
+        stream = self.pa.open(   format = FORMAT,
+                                 channels = CHANNELS,
+                                 rate = RATE,
+                                 input = True,
+                                 # input_device_index = device_index,
+                                 frames_per_buffer = INPUT_FRAMES_PER_BLOCK)
+
+        return stream
+
+    def tapDetected(self):
+        print "Tap!"
+
+    def listen(self):
+        try:
+            block = self.stream.read(INPUT_FRAMES_PER_BLOCK)
+        except IOError, e:
+            # dammit. 
+            self.errorcount += 1
+            print( "(%d) Error recording: %s"%(self.errorcount,e) )
+            self.noisycount = 1
+            return
+
+        amplitude = get_rms( block )
+        if amplitude > self.tap_threshold:
+            # noisy block
+            self.quietcount = 0
+            self.noisycount += 1
+            if self.noisycount > OVERSENSITIVE:
+                # turn down the sensitivity
+                self.tap_threshold *= 1.1
+        else:            
+            # quiet block.
+
+            if 1 <= self.noisycount <= MAX_TAP_BLOCKS:
+                self.tapDetected()
+            self.noisycount = 0
+            self.quietcount += 1
+            if self.quietcount > UNDERSENSITIVE:
+                self.tap_threshold *= 0.9
+
+if __name__ == "__main__":
+    tt = TapTester()
+
+    for i in range(1000):
+        tt.listen()