Module ronin

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