pipette_node.py

Go to the documentation of this file.

#!/usr/bin/env python

# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import roslib ; roslib.load_manifest('pipette_driver')
import rospy
import pipette_driver
from pipette_driver.srv import PipetteAction, PipetteActionRequest, PipetteActionResponse
from diagnostic_msgs.msg import DiagnosticStatus, DiagnosticArray, KeyValue

import sys
import getopt
import threading
from collections import defaultdict

_default_bd_addr = "00:80:98:e6:2b:30"    

"""
usage: %(progname)s [-h] [-a <bd_address]
  Driver node for Viaflo Vision Pipette.  
  Will proform actions based on service calls.
 
  Options:
  -h : show this help
  -a : bluetooth address of pipette.  Uses %(default_bd_addr)s as default
"""

def usage():
    global default_bd_address
    doc_args = {'progname':sys.argv[0], 'default_bd_addr':_default_bd_addr}
    print __doc__ % doc_args


class PipetteNode(object):
    def __init__(self, bd_addr):
        print 'Opening pipette'
        port = 1
        pipette = pipette_driver.ViafloPipette()
        pipette.open(bd_addr, port)
        print 'Requesting info'
        cmd = pipette_driver.ViafloInfoCommand()
        self.pipette_info = pipette.send_msg(cmd)
        print 'Requesting version'
        cmd = pipette_driver.ViafloVersionCommand()
        self.pipette_version = pipette.send_msg(cmd)
        print 'Requesting position'
        cmd = pipette_driver.ViafloPositionQueryCommand()
        self.pipette_position = resp = pipette.send_msg(cmd).pipette_stop
        self.last_action = PipetteActionRequest.QUERY

        self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
        self.level = DiagnosticStatus.OK
        self.message = "OK"
        self.bd_addr = bd_addr
        self.pipette = pipette
        self.quit = False

        # Lock allows sharing between diagnostics thread and sevice callback
        self.lock = threading.Lock()
        # These are the values that the lock protects:
        #   pipette_position
        #   state
        #   msg
        # These value are used only by diagnostics:
        #   pipette_info
        #   pipette_version
        #   bd_addr
        # These values are used only by callback:
        #   pipette        

        # Start diagnostics thread
        self.diagnostic_thread = threading.Thread(None, self.publish_diagnostics)    
        self.diagnostic_thread.start()
        srv = rospy.Service('pipette_action', PipetteAction, self.pipette_action_callback)
        
    def stop(self):
        print "Stopping diagnostics"
        self.quit = True
        self.diagnostic_thread.join()

    def publish_diagnostics(self):
        print "Diagnostics start"
        r = rospy.Rate(1) #1Hz
        action_dict = defaultdict(lambda : "???", {1:'Query', 2:'Purge', 3:'Aspirate', 4:'Dispense'})
        while not self.quit and not rospy.is_shutdown():
            with self.lock:
                pipette_position = self.pipette_position
                level = self.level
                message = self.message
                last_action = self.last_action
            kv = []
            kv.append( KeyValue('Position (uL)', str(pipette_position)))
            kv.append( KeyValue('Last Action',   "%s (%d)" % (action_dict[last_action], last_action)))
            name = "Viaflo Pipette (%s)" % self.bd_addr
            hardware_id = self.pipette_info.serial
            d = DiagnosticStatus(level, name, message, hardware_id, kv)
            h = roslib.msg.Header()
            h.stamp = rospy.Time.now()
            s = DiagnosticArray(h, [d])
            self.diag_pub.publish(s)
            r.sleep()
        print "Diagnostics stop"            

    def pipette_action_callback(self, req):
        print "Action callback"
        action = req.action
        if action==req.PURGE:
            cmd = pipette_driver.ViafloPurgeCommand(req.speed,req.bells)
        elif action==req.DISPENSE:
            cmd = pipette_driver.ViafloDispenseCommand(req.speed,req.amount,req.bells)
        elif action==req.ASPIRATE:
            cmd = pipette_driver.ViafloAspirateCommand(req.speed,req.amount,req.bells)
        else:
            raise RuntimeError("Invalid action (%d)" % action)
        
        try:
            pipette_position = self.pipette.send_msg(cmd).pipette_stop
        except Exception,e:
            with self.lock:
                self.level = DiagnosticStatus.ERROR
                self.message = str(e)
            raise

        # Update pipette diagnostics
        with self.lock:
            self.level = DiagnosticStatus.OK
            self.messge = "OK"
            self.pipette_position = pipette_position
            self.last_action = action

        return PipetteActionResponse(pipette_position)


def main():
    rospy.init_node('pipette_node')
    argv = rospy.myargv(argv=sys.argv)

    # Default values
    port = 1
    bd_addr = _default_bd_addr

    progname = argv[0]
    optlist, args = getopt.getopt(argv[1:], "ha:", ["help","address="])
    for opt,val in optlist:
        if opt=='--help' or opt=='-h':
            usage()
            return
        elif opt=='--address' or opt=='-a':
            bd_addr = val
        else:
            print "Invalid option '%s'"%field
            sys.exit(1)

    print "Usage bluetooth address : %s" % bd_addr
    p = PipetteNode(bd_addr)    
    rospy.spin()
    p.stop()
    

if __name__ == "__main__":
    main()