tester.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Stuart Glaser
00030 
00031 # This is largely copied from bondpy, with hooks that "break" it for testing purposes.
00032 
00033 import threading
00034 import time
00035 import uuid
00036 
00037 import roslib; roslib.load_manifest('test_bond')
00038 import rospy
00039 from bond.msg import *
00040 from test_bond.srv import *
00041 
00042 import atexit
00043 atexit.register(rospy.signal_shutdown, 'exit')
00044 
00045 import BondSM_sm
00046 
00047 class Timeout:
00048     def __init__(self, duration, on_timeout=None):
00049         self.duration = duration
00050         self.timer = threading.Timer(0, self._on_timer)
00051         self.deadline = rospy.Time.now()
00052         self.on_timeout = on_timeout
00053 
00054     def reset(self):
00055         self.timer.cancel()
00056         self.timer = threading.Timer(self.duration.to_sec(), self._on_timer)
00057         self.timer.start()
00058         self.deadline = rospy.Time.now() + self.duration
00059         return self
00060 
00061     def cancel(self):
00062         self.timer.cancel()
00063 
00064     def left(self):
00065         return max(rospy.Duration(0), self.deadline - rospy.Time.now())
00066 
00067     def _on_timer(self):
00068         if self.on_timeout:
00069             self.on_timeout()
00070 
00071 
00072 class BondTester:
00073     def __init__(self, req):
00074         self.req = req
00075         self.topic = req.topic
00076         self.id = req.id
00077         self.instance_id = str(uuid.uuid4())
00078         self.on_death = None
00079         self.on_life = None
00080         self.is_shutdown = False
00081         self.sister_died_first = False
00082         self.death_started = None
00083 
00084         self.sm = BondSM_sm.BondSM_sm(self)
00085         #self.sm.setDebugFlag(True)
00086         self.lock = threading.RLock()
00087         self.condition = threading.Condition(self.lock)
00088 
00089         self.connect_timeout = Timeout(rospy.Duration(Constants.DEFAULT_CONNECT_TIMEOUT), self._on_connect_timeout)
00090         self.heartbeat_timeout = Timeout(rospy.Duration(Constants.DEFAULT_HEARTBEAT_TIMEOUT), self._on_heartbeat_timeout)
00091         self.disconnect_timeout = Timeout(rospy.Duration(Constants.DEFAULT_DISCONNECT_TIMEOUT), self._on_disconnect_timeout)
00092 
00093         self.connect_timeout.reset()
00094 
00095         self.sub = rospy.Subscriber(self.topic, Status, self._on_bond_status)
00096         self.pub = rospy.Publisher(self.topic, Status)
00097 
00098         self.thread = threading.Thread(target=self._publishing_thread)
00099         self.thread.daemon = True
00100         self.thread.start()
00101 
00102         if req.delay_death >= rospy.Duration(0.0):
00103             self.death_timeout = Timeout(req.delay_death, self.die).reset()
00104 
00105     def _on_connect_timeout(self):
00106         with self.lock:
00107             self.sm.ConnectTimeout()
00108     def _on_heartbeat_timeout(self):
00109         if self.req.inhibit_death:
00110             return
00111         with self.lock:
00112             self.sm.HeartbeatTimeout()
00113     def _on_disconnect_timeout(self):
00114         with self.lock:
00115             self.sm.DisconnectTimeout()
00116 
00117     def __del__(self):
00118         self.shutdown()
00119 
00120     def shutdown(self):
00121         if not self.is_shutdown:
00122             with self.lock:
00123                 self.is_shutdown = True
00124                 if self.sm.getState().getName() != 'SM.Dead':
00125                     print "I'm not dead yet:", self.id, " in ", self.sm.getState().getName()
00126                     self.die()
00127                 self.sub.unregister()
00128                 self.pub.unregister()
00129                 self.condition.notify_all()
00130                 print "Unregistered"
00131 
00132     def _on_bond_status(self, msg):
00133         # Filters out messages from other bonds and messages from ourselves
00134         if msg.id == self.id and msg.instance_id != self.instance_id:
00135             with self.lock:
00136                 if msg.active:
00137                     if not self.is_dead():
00138                         self.sm.SisterAlive()
00139                 else:
00140                     if self.req.inhibit_death:
00141                         return
00142 
00143                     self.sm.SisterDead()
00144 
00145                     # Immediate ack for sister's death notification
00146                     if self.sister_died_first:
00147                         self._publish(False)
00148 
00149 
00150     def _publish(self, active):
00151         msg = Status()
00152         msg.header.stamp = rospy.Time.now()
00153         msg.id = self.id
00154         msg.instance_id = self.instance_id
00155         msg.active = active
00156 
00157         if not msg.active and self.req.inhibit_death_message:
00158             pass
00159         else:
00160             self.pub.publish(msg)
00161 
00162     def _publishing_thread(self):
00163 
00164         time.sleep(self.req.delay_connect.to_sec())
00165 
00166         with self.lock:
00167             # Publishing ALIVE
00168             while not self.is_shutdown and self.sm.getState().getName() in ['SM.WaitingForSister', 'SM.Alive']:
00169                 self._publish(True)
00170                 self.condition.wait(Constants.DEFAULT_HEARTBEAT_PERIOD)
00171 
00172             # Publishing DEAD
00173             while not self.is_shutdown and self.sm.getState().getName() == 'SM.AwaitSisterDeath':
00174                 self._publish(False)
00175                 self.condition.wait(Constants.DEAD_PUBLISH_PERIOD)
00176 
00177 
00178     def Connected(self):
00179         self.connect_timeout.cancel()
00180         self.condition.notify_all()
00181         if self.on_life:
00182             self.on_life()
00183 
00184     def Heartbeat(self):
00185         self.heartbeat_timeout.reset()
00186 
00187     def SisterDied(self):
00188         self.sister_died_first = True
00189 
00190     def Death(self):
00191         self.condition.notify_all()
00192         self.heartbeat_timeout.cancel()
00193         self.disconnect_timeout.cancel()
00194         if not self.death_started:
00195             self.death_started = rospy.Time.now()
00196 
00197     def StartDying(self):
00198         self.heartbeat_timeout.cancel()
00199         self.disconnect_timeout.reset()
00200         if not self.death_started:
00201             self.death_started = rospy.Time.now()
00202 
00203     def wait_for_life(self, timeout = None):
00204         deadline = timeout and Timeout(timeout).reset()
00205         with self.lock:
00206             while self.sm.getState().getName() == 'SM.WaitingForSister':
00207                 if deadline and deadline.left() == rospy.Duration(0):
00208                     break
00209                 self.condition.wait(deadline and deadline.left().to_sec())
00210             return self.sm.getState().getName() != 'SM.WaitingForSister'
00211 
00212     def wait_for_death(self, timeout = None):
00213         deadline = timeout and Timeout(timeout).reset()
00214         with self.lock:
00215             while self.sm.getState().getName() != 'SM.Dead':
00216                 if deadline and deadline.left() == rospy.Duration(0):
00217                     break
00218                 self.condition.wait(deadline and deadline.left().to_sec())
00219             return self.sm.getState().getName() == 'SM.Dead'
00220 
00221     def is_dead(self):
00222         with self.lock:
00223             return self.sm.getState().getName() == 'SM.Dead'
00224 
00225     def die(self):
00226         with self.lock:
00227             self.sm.Die()
00228             self._publish(False)
00229 
00230     def __repr__(self):
00231         return "[Bond %s, Instance %s (%s)]" % (self.id, self.instance_id, self.sm.getState().getName())
00232 
00233 
00234 class Tester:
00235     def __init__(self):
00236         self.bond_tester = None
00237         self.service = rospy.Service('test_bond', TestBond, self._test_bond)
00238 
00239     def _test_bond(self, req):
00240         print "TEST"
00241         if self.bond_tester:
00242             self.bond_tester.shutdown()
00243         self.bond_tester = BondTester(req)
00244         print "Test bond instance id: %s" % self.bond_tester.instance_id
00245         return TestBondResponse()
00246 
00247 def main():
00248     rospy.init_node('bond_tester', anonymous=True, disable_signals=True)
00249     tester = Tester()
00250     rospy.spin()
00251 
00252 if __name__ == '__main__': main()


test_bond
Author(s): Stuart Glaser
autogenerated on Sun Oct 5 2014 22:32:35