Source Code for Module ronin

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2010, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32  # 
 33  # Revision $Id: graph.py 8649 2010-03-12 19:05:53Z kwc $ 
 34  """ 
 35  Master-less rospy node api. 
 36   
 37  The ronin API is more handle-based than the rospy API. This is in 
 38  order to provide better forwards compatibility for ronin as changes to 
 39  rospy are made to enable this functionality more gracefully. 
 40   
 41  Here is an example ronin-based talker::: 
 42   
 43      n = ronin.init_node('talker', anonymous=True) 
 44      pub = n.Publisher('chatter', String) 
 45      r = n.Rate(10) # 10hz 
 46      while not n.is_shutdown(): 
 47          str = "hello world %s"%n.get_time() 
 48          n.loginfo(str) 
 49          pub.publish(str) 
 50          r.sleep() 
 51   
 52  Authors: Ken Conley and Blaise Gassend 
 53  """ 
 54   
 55  import roslib; roslib.load_manifest('ronin') 
 56   
 57  import rospy 
 58   
 59  import roslib.masterapi 
 60   
 61  import rospy.masterslave 
 62  import rospy.registration 
 63   
 64  # hot patch: convince node api that it's already registered 
 65  rospy.masterslave.ROSHandler._is_registered = lambda x: True 
 66   
 67  _MASTER_CHECK_INTERVAL = 10 # seconds 
 68   
 69 -def init_node(name, **kwds): 
 70      kwds['disable_rosout'] = True 
 71      kwds['disable_rostime'] = True 
 72       
 73      rospy.init_node(name, **kwds) 
 74      handle = RoninHandle(name, rospy.get_node_uri(), rospy.Duration(_MASTER_CHECK_INTERVAL)) 
 75       
 76      # hot patch 
 77      rospy.registration.RegManager.reg_added = handle._reg_added 
 78   
 79      handle._check_master() 
 80      return handle 
 81   
 82   
 83 -def _checked_publish(self, *args, **kwds): 
 84      self.ronin._check_master() 
 85      self.ronin_publish(*args[1:], **kwds) 
 86   
 87 -class _PubHandle(object): 
 88 -    def __init__(self, pub, handle): 
 89          self.pub = pub 
 90          self.handle = handle 
 91           
 92 -    def __call__(self, *args, **kwds): 
 93          self.handle._check_master() 
 94          self.pub.ronin_publish(*args, **kwds) 
 95       
 96 -class RoninHandle(object): 
 97   
 98 -    def __init__(self, name, uri, check_interval): 
 99          self._uri = uri 
100          self._name = name 
101          self._check_interval = check_interval 
102          # XMLRPC is stateless, so we can go-ahead and init 
103          self._master = roslib.masterapi.Master(name) 
104   
105          self._pub_list = [] 
106          self._sub_list = [] 
107           
108          self._next_check_time = None 
109          self._master_pid = None 
110   
111          self.Rate = rospy.Rate 
112          self.Subscriber = rospy.Subscriber 
113   
114          self.is_shutdown = rospy.is_shutdown 
115          self.get_time = rospy.get_time 
116          self.get_rostime = rospy.get_rostime 
117          self.sleep = rospy.sleep 
118           
119          self.loginfo = rospy.loginfo 
120          self.logerr = rospy.logerr         
121          self.logdebug = rospy.logdebug 
122          self.logwarn = rospy.logwarn         
123           
124 -    def _reg_added(self, resolved_name, data_type_or_uri, reg_type): 
125          try: 
126              if reg_type == rospy.registration.Registration.PUB: 
127                  self._pub_list.append((resolved_name, data_type_or_uri)) 
128                  if self._master_pid is not None: 
129                      master.registerPublisher(resolved_name, data_type_or_uri, our_uri) 
130              elif reg_type == rospy.registration.Registration.SUB: 
131                  self._sub_list.append((resolved_name, data_type_or_uri)) 
132                  if self._master_pid is not None: 
133                      master.registerSubscriber(resolved_name, data_type_or_uri, our_uri) 
134          except: 
135              pass 
136          return True 
137   
138 -    def _checked_publish(self, *args, **nargs): 
139          self.ronin._check_master() 
140          original_Publisher_publish(*args, **nargs) 
141   
142 -    def Publisher(self, *args, **kwds): 
143          # wrap the publisher with a call to check_master before actually publishing 
144          pub = rospy.Publisher(*args, **kwds) 
145          pub.ronin_publish = pub.publish 
146          pub.publish = _PubHandle(pub, self) 
147          return pub 
148   
149 -    def _check_master(self): 
150          # NOTE: it's not possible for a ronin node to be on simtime, 
151          # so this is just wallclock convenience. 
152          now = rospy.Time.now() 
153          next_check_time = self._next_check_time 
154          if next_check_time == None or now > next_check_time: 
155              self._next_check_time = now + self._check_interval 
156              if self._master_pid != None: 
157                  try: 
158                      new_pid = master.getPid() 
159                      if new_pid != self._master_pid: 
160                          self._master_pid = None 
161                  except: 
162                      self._master_pid = None 
163              if self._master_pid == None: 
164                  try: 
165                      for name, type in self._pub_list: 
166                          self._master.registerPublisher(name, type, self._uri) 
167                      for name, type in self._sub_list: 
168                          self._master.registerSubscriber(name, type, self._uri) 
169   
170                      self._master_pid = self._master.getPid() 
171                  except Exception, e: 
172                      pass 
173          if self._master_pid: 
174              return True 
175