configure_devices.py

Go to the documentation of this file.

#! /usr/bin/env python

import os
import sys
import fcntl
import select
import time
from glob import glob
import collections
import yaml

import roslib

class DeviceNotDetected(Exception): pass

def open_nb(name, mode = 'r'):
    f = open(name, mode, 0)
    fcntl.fcntl(f, fcntl.F_SETFL, os.O_NONBLOCK)
    return f

def get_hidraw():
    return glob('/dev/hidraw*')
def get_js():
    return glob('/dev/input/js*')


def what_moved(paths, timeout = 1.0):
    fs = [open_nb(p, 'rb') for p in paths]
    started = time.time()
    moved = collections.defaultdict(int)
    while started + timeout >= time.time():
        ready, _, _ = select.select(fs, [], [], 0.25)
        for f in ready:
            buf = f.read(10000)
            moved[f.name] += len(buf)
        time.sleep(0.001)
    #print "## Moved:", moved
    sys.stdout.flush()
    return dict(moved)


def find_device(description, candidates, timeout = -1):
    print "Getting a baseline..."
    sys.stdout.flush()
    baseline = what_moved(candidates)
    print "Ok\n"
    print "Start pushing the %s" % description
    sys.stdout.flush()
    started = time.time()
    while timeout < 0 or time.time() < started + timeout:
        moved = what_moved(candidates, 0.2)

        began = []
        increased = []
        for m in moved:
            if m not in baseline:
                began.append(m)
            elif moved[m] > 3 * baseline[m]:
                increased.append(m)
                
        #print "## Began: %s" % began
        #print "## Increased: %s" % increased
        
        if len(began) > 1:
            raise DeviceNotDetected("Devices noticed were: %s" % ', '.join(began))
        elif len(began) == 1:
            print "Found: %s" % began[0]
            time.sleep(1.0)
            return began[0]
        elif len(increased) == 1:
            print "Found: %s" % increased[0]
            time.sleep(1.0)
            return increased[0]
    raise DeviceNotDetected("No devices noticed")

def find_device_robust(description, candidates):
    while True:
        try:
            return find_device(description, candidates, 5.0)
        except DeviceNotDetected, ex:
            print "Failed:", ex
            print "."
            time.sleep(1)
            print "Trying again"

os.system("sudo chmod a+rwx /dev/hidraw*")
os.system("sudo chmod a+rwx /dev/input/js*")
os.system("sudo chmod a+rwx -R /dev/bus/usb/*")

y = {
    'l_pedals': {'device': find_device_robust('left transcription pedals', get_hidraw())},
    'r_pedals': {'device': find_device_robust('right transcription pedals', get_hidraw())},
    'l_rudder_pedals': {'dev': find_device_robust('left rudder pedals', get_js())},
    'r_rudder_pedals': {'dev': find_device_robust('right rudder pedals', get_js())},
}


filename = os.path.join(roslib.packages.get_pkg_dir('teleop_microscribe'), 'devices.yaml')
with open(filename, 'w') as fout:
    yaml.dump(y, fout)
    print "Written to: %s" % filename