nao_tactile.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to read Nao's bumpers and tactile sensors
00005 # This code is currently compatible to NaoQI version 1.6
00006 #
00007 # Copyright 2010 Stefan Osswald, University of Freiburg
00008 # http://www.ros.org/wiki/nao
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions are met:
00012 #
00013 #    # Redistributions of source code must retain the above copyright
00014 #       notice, this list of conditions and the following disclaimer.
00015 #    # Redistributions in binary form must reproduce the above copyright
00016 #       notice, this list of conditions and the following disclaimer in the
00017 #       documentation and/or other materials provided with the distribution.
00018 #    # Neither the name of the University of Freiburg nor the names of its
00019 #       contributors may be used to endorse or promote products derived from
00020 #       this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 import rospy
00036 
00037 import naoqi
00038 from nao_msgs.msg import TactileTouch, Bumper
00039 from std_msgs.msg import Bool
00040 from nao_driver import NaoNode
00041 from naoqi import ( ALModule, ALBroker, ALProxy )
00042 
00043 #
00044 # Notes:
00045 # - A port number > 0 for the module must be specified.
00046 #   If port 0 is used, a free port will be assigned automatically,
00047 #   but naoqi is unable to pick up the assigned port number, leaving
00048 #   the module unable to communicate with naoqi (1.10.52).
00049 # 
00050 # - Callback functions _must_ have a docstring, otherwise they won't get bound.
00051 # 
00052 # - Shutting down the broker manually will result in a deadlock,
00053 #   not shutting down the broker will sometimes result in an exception 
00054 #   when the script ends (1.10.52).
00055 #
00056 
00057 class NaoTactile(ALModule):
00058     "Sends callbacks for tactile touch, bumper press and foot contact to ROS"
00059     def __init__(self, moduleName):
00060         # get connection from command line:
00061         from optparse import OptionParser
00062 
00063         parser = OptionParser()
00064         parser.add_option("--ip", dest="ip", default="",
00065                           help="IP/hostname of broker. Default is system's default IP address.", metavar="IP")
00066         parser.add_option("--port", dest="port", default=0,
00067                           help="IP/hostname of broker. Default is automatic port.", metavar="PORT")
00068         parser.add_option("--pip", dest="pip", default="127.0.0.1",
00069                           help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP")
00070         parser.add_option("--pport", dest="pport", default=9559,
00071                           help="port of parent broker. Default is 9559.", metavar="PORT")
00072 
00073         (options, args) = parser.parse_args()
00074         self.ip = options.ip
00075         self.port = int(options.port)
00076         self.pip = options.pip
00077         self.pport = int(options.pport)
00078         self.moduleName = moduleName
00079         
00080         self.init_almodule()
00081         
00082         # ROS initialization:
00083         rospy.init_node('nao_tactile')
00084              
00085         # init. messages:
00086         self.tactile = TactileTouch()              
00087         self.bumper = Bumper()
00088         self.tactilePub = rospy.Publisher("tactile_touch", TactileTouch, queue_size=10)
00089         self.bumperPub = rospy.Publisher("bumper", Bumper, queue_size=10)
00090         
00091         try:
00092             footContact = self.memProxy.getData("footContact", 0)
00093         except RuntimeError:
00094             footContact = None
00095             
00096         if footContact is None:
00097             self.hasFootContactKey = False
00098             rospy.loginfo("Foot contact key is not present in ALMemory, will not publish to foot_contact topic.")
00099         else:
00100             self.hasFootContactKey = True
00101             self.footContactPub = rospy.Publisher("foot_contact", Bool, latch=True, queue_size=10)
00102             self.footContactPub.publish(footContact > 0.0)
00103 
00104         # constants in TactileTouch and Bumper will not be available in callback functions
00105         # as they are executed in the parent broker context (i.e. on robot),
00106         # so they have to be copied to member variables
00107         self.tactileTouchFrontButton = TactileTouch.buttonFront;
00108         self.tactileTouchMiddleButton = TactileTouch.buttonMiddle;
00109         self.tactileTouchRearButton = TactileTouch.buttonRear;
00110         self.bumperRightButton = Bumper.right;
00111         self.bumperLeftButton = Bumper.left;
00112         
00113         self.subscribe()
00114         
00115         rospy.loginfo("nao_tactile initialized")
00116 
00117     def init_almodule(self):
00118         # before we can instantiate an ALModule, an ALBroker has to be created
00119         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00120         try:
00121             self.broker = ALBroker("%sBroker" % self.moduleName, self.ip, self.port, self.pip, self.pport)
00122         except RuntimeError,e:
00123             print("Could not connect to NaoQi's main broker")
00124             exit(1)
00125         ALModule.__init__(self, self.moduleName)
00126         
00127         self.memProxy = ALProxy("ALMemory",self.pip,self.pport)
00128         # TODO: check self.memProxy.version() for > 1.6
00129         if self.memProxy is None:
00130             rospy.logerror("Could not get a proxy to ALMemory on %s:%d", self.pip, self.pport)
00131             exit(1)
00132 
00133 
00134     def shutdown(self): 
00135         self.unsubscribe()
00136         # Shutting down broker seems to be not necessary any more
00137         # try:
00138         #     self.broker.shutdown()
00139         # except RuntimeError,e:
00140         #     rospy.logwarn("Could not shut down Python Broker: %s", e)
00141 
00142 
00143     def subscribe(self):
00144         self.memProxy.subscribeToEvent("FrontTactilTouched", self.moduleName, "onTactileChanged")
00145         self.memProxy.subscribeToEvent("MiddleTactilTouched", self.moduleName, "onTactileChanged")
00146         self.memProxy.subscribeToEvent("RearTactilTouched", self.moduleName, "onTactileChanged")
00147         self.memProxy.subscribeToEvent("RightBumperPressed", self.moduleName, "onBumperChanged")
00148         self.memProxy.subscribeToEvent("LeftBumperPressed", self.moduleName, "onBumperChanged")
00149         if self.hasFootContactKey:
00150             self.memProxy.subscribeToEvent("footContactChanged", self.moduleName, "onFootContactChanged")
00151 
00152 
00153     def unsubscribe(self):
00154         self.memProxy.unsubscribeToEvent("FrontTactilTouched", self.moduleName)
00155         self.memProxy.unsubscribeToEvent("MiddleTactilTouched", self.moduleName)
00156         self.memProxy.unsubscribeToEvent("RearTactilTouched", self.moduleName)
00157         self.memProxy.unsubscribeToEvent("RightBumperPressed", self.moduleName)
00158         self.memProxy.unsubscribeToEvent("LeftBumperPressed", self.moduleName)
00159         if self.hasFootContactKey:
00160             self.memProxy.unsubscribeToEvent("footContactChanged", self.moduleName)
00161 
00162 
00163     def onTactileChanged(self, strVarName, value, strMessage):
00164         "Called when tactile touch status changes in ALMemory"
00165         if strVarName == "FrontTactilTouched":
00166             self.tactile.button = self.tactileTouchFrontButton
00167         elif strVarName == "MiddleTactilTouched":
00168             self.tactile.button = self.tactileTouchMiddleButton
00169         elif strVarName == "RearTactilTouched":
00170             self.tactile.button = self.tactileTouchRearButton
00171 
00172         self.tactile.state = int(value);
00173         self.tactilePub.publish(self.tactile)
00174         rospy.logdebug("tactile touched: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
00175 
00176     def onBumperChanged(self, strVarName, value, strMessage):
00177         "Called when bumper status changes in ALMemory"
00178         if strVarName == "RightBumperPressed":
00179             self.bumper.bumper = self.bumperRightButton
00180         elif strVarName == "LeftBumperPressed":
00181             self.bumper.bumper = self.bumperLeftButton
00182 
00183         self.bumper.state = int(value);
00184         self.bumperPub.publish(self.bumper)
00185         rospy.logdebug("bumper pressed: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
00186         
00187     def onFootContactChanged(self, strVarName, value, strMessage):
00188         "Called when foot contact changes in ALMemory"
00189         self.footContactPub.publish(value > 0.0)
00190 
00191 if __name__ == '__main__':
00192     ROSNaoTactileModule = NaoTactile("ROSNaoTactileModule")
00193 
00194     rospy.spin()
00195     
00196     rospy.loginfo("Stopping nao_tactile ...")
00197     ROSNaoTactileModule.shutdown();        
00198     rospy.loginfo("nao_tactile stopped.")
00199     exit(0)


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04