Refactor code into a Python module

CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS
   catkin_virtualenv
   dynamic_reconfigure)
 
+catkin_python_setup()
+
 generate_dynamic_reconfigure_options(
   cfg/Respeaker.cfg)
 

scripts/respeaker_gencfg.py

@@ -4,11 +4,12 @@
 
 import os
 import sys
-from respeaker_node import PARAMETERS, init_respeaker
+from respeaker_ros.parameters import PARAMETERS
+from respeaker_ros import RespeakerInterface
 
 
 def main(out):
-    dev = init_respeaker()
+    dev = RespeakerInterface()
     if not dev:
         print('No device found. Please connect a device.')
         return
@@ -44,15 +45,15 @@ def main(out):
                     def_ = False
                 f.write("""
 gen.add("{name}", bool_t, 0, "{desc}", {def_})""".format(
-    name=key, desc=desc_, def_=def_))
+                    name=key, desc=desc_, def_=def_))
             elif type_ == "int":
                 f.write("""
 gen.add("{name}", int_t, 0, "{desc}", {def_}, {min_}, {max_})""".format(
-    name=key, desc=desc_, def_=def_, min_=min_, max_=max_))
+                    name=key, desc=desc_, def_=def_, min_=min_, max_=max_))
             elif type_ == "float":
                 f.write("""
 gen.add("{name}", double_t, 0, "{desc}", {def_:f}, {min_:f}, {max_:f})""".format(
-    name=key, desc=desc_, def_=def_, min_=min_, max_=max_))
+                    name=key, desc=desc_, def_=def_, min_=min_, max_=max_))
             else:
                 print "Param '{name}' is ignored.".format(name=key)
 

setup.py

@@ -0,0 +1,11 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['respeaker_ros'],
+    package_dir={'': 'src'})
+
+setup(**setup_args)

src/respeaker_ros/__init__.py

@@ -0,0 +1,3 @@
+from .audio import RespeakerAudio
+from .interface import RespeakerInterface
+from .node import RespeakerNode

src/respeaker_ros/audio.py

@@ -0,0 +1,90 @@
+from .util import ignore_stderr
+import pyaudio
+import rospy
+import numpy as np
+
+
+class RespeakerAudio(object):
+    def __init__(self, on_audio, channels=None, suppress_error=True):
+        self.on_audio = on_audio
+        with ignore_stderr(enable=suppress_error):
+            self.pyaudio = pyaudio.PyAudio()
+        self.available_channels = None
+        self.channels = channels
+        self.device_index = None
+        self.rate = 16000
+        self.bitwidth = 2
+        self.bitdepth = 16
+
+        # find device
+        count = self.pyaudio.get_device_count()
+        rospy.logdebug("%d audio devices found" % count)
+        for i in range(count):
+            info = self.pyaudio.get_device_info_by_index(i)
+            name = info["name"].encode("utf-8")
+            chan = info["maxInputChannels"]
+            rospy.logdebug(" - %d: %s" % (i, name))
+            if name.lower().find("respeaker") >= 0:
+                self.available_channels = chan
+                self.device_index = i
+                rospy.loginfo("Found %d: %s (channels: %d)" % (i, name, chan))
+                break
+        if self.device_index is None:
+            rospy.logwarn("Failed to find respeaker device by name. Using default input")
+            info = self.pyaudio.get_default_input_device_info()
+            self.available_channels = info["maxInputChannels"]
+            self.device_index = info["index"]
+
+        if self.available_channels != 6:
+            rospy.logwarn("%d channel is found for respeaker" % self.available_channels)
+            rospy.logwarn("You may have to update firmware.")
+        if self.channels is None:
+            self.channels = range(self.available_channels)
+        else:
+            self.channels = filter(lambda c: 0 <= c < self.available_channels, self.channels)
+        if not self.channels:
+            raise RuntimeError('Invalid channels %s. (Available channels are %s)' % (
+                self.channels, self.available_channels))
+        rospy.loginfo('Using channels %s' % self.channels)
+
+        self.stream = self.pyaudio.open(
+            input=True, start=False,
+            format=pyaudio.paInt16,
+            channels=self.available_channels,
+            rate=self.rate,
+            frames_per_buffer=1024,
+            stream_callback=self.stream_callback,
+            input_device_index=self.device_index,
+        )
+
+    def __del__(self):
+        self.stop()
+        try:
+            self.stream.close()
+        except:
+            pass
+        finally:
+            self.stream = None
+        try:
+            self.pyaudio.terminate()
+        except:
+            pass
+
+    def stream_callback(self, in_data, frame_count, time_info, status):
+        # split channel
+        data = np.fromstring(in_data, dtype=np.int16)
+        chunk_per_channel = len(data) / self.available_channels
+        data = np.reshape(data, (chunk_per_channel, self.available_channels))
+        for chan in self.channels:
+            chan_data = data[:, chan]
+            # invoke callback
+            self.on_audio(chan_data.tostring(), chan)
+        return None, pyaudio.paContinue
+
+    def start(self):
+        if self.stream.is_stopped():
+            self.stream.start_stream()
+
+    def stop(self):
+        if self.stream.is_active():
+            self.stream.stop_stream()

src/respeaker_ros/interface.py

@@ -0,0 +1,122 @@
+import rospy
+import time
+import struct
+import usb.core
+import usb.util
+
+from respeaker_ros.parameters import PARAMETERS
+
+
+class RespeakerInterface(object):
+    VENDOR_ID = 0x2886
+    PRODUCT_ID = 0x0018
+    TIMEOUT = 100000
+
+    def __init__(self):
+        try:
+            from pixel_ring import usb_pixel_ring_v2
+        except IOError as e:
+            print e
+            raise RuntimeError("Check the device is connected and recognized")
+
+        self.dev = usb.core.find(idVendor=self.VENDOR_ID,
+                                 idProduct=self.PRODUCT_ID)
+        if not self.dev:
+            raise RuntimeError("Failed to find Respeaker device")
+        rospy.loginfo("Initializing Respeaker device")
+        self.dev.reset()
+        self.pixel_ring = usb_pixel_ring_v2.PixelRing(self.dev)
+        self.set_led_think()
+        time.sleep(10)  # it will take 10 seconds to re-recognize as audio device
+        self.set_led_trace()
+        rospy.loginfo("Respeaker device initialized (Version: %s)" % self.version)
+
+    def __del__(self):
+        try:
+            self.close()
+        except:
+            pass
+        finally:
+            self.dev = None
+
+    def write(self, name, value):
+        try:
+            data = PARAMETERS[name]
+        except KeyError:
+            return
+
+        if data[5] == 'ro':
+            raise ValueError('{} is read-only'.format(name))
+
+        id = data[0]
+
+        # 4 bytes offset, 4 bytes value, 4 bytes type
+        if data[2] == 'int':
+            payload = struct.pack(b'iii', data[1], int(value), 1)
+        else:
+            payload = struct.pack(b'ifi', data[1], float(value), 0)
+
+        self.dev.ctrl_transfer(
+            usb.util.CTRL_OUT | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+            0, 0, id, payload, self.TIMEOUT)
+
+    def read(self, name):
+        try:
+            data = PARAMETERS[name]
+        except KeyError:
+            return
+
+        id = data[0]
+
+        cmd = 0x80 | data[1]
+        if data[2] == 'int':
+            cmd |= 0x40
+
+        length = 8
+
+        response = self.dev.ctrl_transfer(
+            usb.util.CTRL_IN | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+            0, cmd, id, length, self.TIMEOUT)
+
+        response = struct.unpack(b'ii', response.tostring())
+
+        if data[2] == 'int':
+            result = response[0]
+        else:
+            result = response[0] * (2. ** response[1])
+
+        return result
+
+    def set_led_think(self):
+        self.pixel_ring.set_brightness(10)
+        self.pixel_ring.think()
+
+    def set_led_trace(self):
+        self.pixel_ring.set_brightness(20)
+        self.pixel_ring.trace()
+
+    def set_led_color(self, r, g, b, a):
+        self.pixel_ring.set_brightness(int(20 * a))
+        self.pixel_ring.set_color(r=int(r * 255), g=int(g * 255), b=int(b * 255))
+
+    def set_vad_threshold(self, db):
+        self.write('GAMMAVAD_SR', db)
+
+    def is_voice(self):
+        return self.read('VOICEACTIVITY')
+
+    @property
+    def direction(self):
+        return self.read('DOAANGLE')
+
+    @property
+    def version(self):
+        return self.dev.ctrl_transfer(
+            usb.util.CTRL_IN | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+            0, 0x80, 0, 1, self.TIMEOUT)[0]
+
+    def close(self):
+        """
+        close the interface
+        """
+        usb.util.dispose_resources(self.dev)