From 66f7a3f0be3060f632385bac3384d5fc9e98cdd7 Mon Sep 17 00:00:00 2001 From: Miguel Sarabia Date: Sun, 4 Aug 2013 12:45:55 +0100 Subject: [PATCH 1/3] Initial test for deadlock in simple_action_server.py --- test/CMakeLists.txt | 1 + ...simple_action_server_deadlock_companion.py | 95 ++++++++++++ test/test_simple_action_server_deadlock.py | 137 ++++++++++++++++++ ...imple_action_server_deadlock_python.launch | 11 ++ 4 files changed, 244 insertions(+) create mode 100755 test/simple_action_server_deadlock_companion.py create mode 100755 test/test_simple_action_server_deadlock.py create mode 100644 test/test_simple_action_server_deadlock_python.launch diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7d4293d8..4add4fdb 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -62,6 +62,7 @@ add_rostest(test_python_server2.launch) add_rostest(test_python_server3.launch) add_rostest(test_python_simple_server.launch) add_rostest(test_exercise_simple_clients.launch) +add_rostest(test_simple_action_server_deadlock_python.launch) catkin_add_gtest(actionlib-destruction_guard_test destruction_guard_test.cpp) if(TARGET actionlib-destruction_guard_test) diff --git a/test/simple_action_server_deadlock_companion.py b/test/simple_action_server_deadlock_companion.py new file mode 100755 index 00000000..1a74077a --- /dev/null +++ b/test/simple_action_server_deadlock_companion.py @@ -0,0 +1,95 @@ +#! /usr/bin/env python +# +# +# Copyright (c) 2013, Miguel Sarabia +# Imperial College London +# 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 Imperial College London 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. +# + +#=============================================================================== +# CONSTANTS +#=============================================================================== +class Constants: + pkg = "actionlib" + node = "simple_action_server_deadlock_companion" + topic = "deadlock" + + max_action_duration = 5; + min_action_duration = 0.1; + +#=============================================================================== +# IMPORTS +#=============================================================================== +import roslib; roslib.load_manifest(Constants.pkg) +import random +import rospy +import actionlib +from actionlib.msg import TestAction, TestGoal +from actionlib_msgs.msg import GoalStatus + +#=============================================================================== +# COMPANION CLASS +#=============================================================================== +class DeadlockCompanion: + + def __init__(self): + #Seed random with fully resolved name of node and current time + random.seed( rospy.get_name() + str(rospy.Time.now().to_sec()) ) + + #Create actionlib client + self.action_client = actionlib.SimpleActionClient( + Constants.topic, + TestAction) + + def run(self): + while not rospy.is_shutdown(): + # Send dummy goal + self.action_client.send_goal( TestGoal() ) + + #Wait for a random amount of time + action_duration = random.uniform(0, Constants.max_action_duration) + self.action_client.wait_for_result( rospy.Duration(action_duration) ) + + state = self.action_client.get_state() + if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING : + self.action_client.cancel_goal() + + +#=============================================================================== +# MAIN +#=============================================================================== +if __name__ == '__main__': + rospy.init_node(Constants.node) + try: + companion = DeadlockCompanion() + companion.run() + except (KeyboardInterrupt, SystemExit): + raise + except: + pass + + diff --git a/test/test_simple_action_server_deadlock.py b/test/test_simple_action_server_deadlock.py new file mode 100755 index 00000000..364201c2 --- /dev/null +++ b/test/test_simple_action_server_deadlock.py @@ -0,0 +1,137 @@ +#! /usr/bin/env python +# +# +# Copyright (c) 2013, Miguel Sarabia +# Imperial College London +# 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 Imperial College London 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. +# + +#=============================================================================== +# CONSTANTS +#=============================================================================== +class Constants: + pkg = "actionlib" + node = "test_simple_action_server_deadlock" + topic = "deadlock" + + deadlock_timeout = 45 # in seconds + shutdown_timeout = 2 # in seconds + max_action_duration = 5; + +#=============================================================================== +# IMPORTS +#=============================================================================== +import roslib; roslib.load_manifest(Constants.pkg) +import sys +import random +import threading +import unittest +import rospy +import rosnode +import actionlib +from actionlib.msg import TestAction + + +#=============================================================================== +# TEST CLASS +#=============================================================================== +class DeadlockTest(unittest.TestCase): + + def test_deadlock(self): + #Prepare condition (for safe preemption) + self.condition = threading.Condition() + self.last_execution_time = None + + # Prepare Simple Action Server + self.action_server = actionlib.SimpleActionServer( + Constants.topic, + TestAction, + execute_cb = self.execute_callback, + auto_start = False) + + self.action_server.register_preempt_callback(self.preempt_callback) + self.action_server.start() + + #Sleep for the amount specified + rospy.sleep(Constants.deadlock_timeout); + + #Start actual tests + running_nodes = set(rosnode.get_node_names()) + required_nodes = { + "/deadlock_companion_1", + "/deadlock_companion_2", + "/deadlock_companion_3", + "/deadlock_companion_4", + "/deadlock_companion_5" } + + self.assertTrue( required_nodes.issubset(running_nodes), + "Required companion nodes are not currently running") + + # Shutdown companions so that we can exit nicely + termination_time = rospy.Time.now() + rosnode.kill_nodes(required_nodes) + + rospy.sleep(Constants.shutdown_timeout) + + #Check last execution wasn't too long ago... + self.assertIsNotNone( self.last_execution_time is None, + "Execute Callback was never executed") + + time_since_last_execution = ( + termination_time - self.last_execution_time).to_sec() + + self.assertTrue( + time_since_last_execution < 2 * Constants.max_action_duration, + "Too long since last goal was executed; likely due to a deadlock" ) + + def execute_callback(self, goal): + #Note down last_execution time + self.last_execution_time = rospy.Time.now() + + #Determine duration of this action + action_duration = random.uniform(0, Constants.max_action_duration) + + with self.condition: + if not self.action_server.is_preempt_requested(): + self.condition.wait(action_duration) + + if self.action_server.is_preempt_requested(): + self.action_server.set_preempted() + else: + self.action_server.set_succeeded() + + def preempt_callback(self): + with self.condition: + self.condition.notify() + +#=============================================================================== +# MAIN +#=============================================================================== +if __name__ == '__main__': + import rostest + rospy.init_node( Constants.node ) + rostest.rosrun( Constants.pkg, Constants.node, DeadlockTest ) diff --git a/test/test_simple_action_server_deadlock_python.launch b/test/test_simple_action_server_deadlock_python.launch new file mode 100644 index 00000000..f5940d1b --- /dev/null +++ b/test/test_simple_action_server_deadlock_python.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + From 7c1556724c65f4670abc55702c26d7994809bc15 Mon Sep 17 00:00:00 2001 From: Miguel Sarabia Date: Sun, 4 Aug 2013 13:46:25 +0100 Subject: [PATCH 2/3] Small tweaks to simple_action_server.py deadlock test --- test/simple_action_server_deadlock_companion.py | 6 ++---- test/test_simple_action_server_deadlock.py | 6 +++--- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/test/simple_action_server_deadlock_companion.py b/test/simple_action_server_deadlock_companion.py index 1a74077a..e2821ead 100755 --- a/test/simple_action_server_deadlock_companion.py +++ b/test/simple_action_server_deadlock_companion.py @@ -38,8 +38,7 @@ class Constants: node = "simple_action_server_deadlock_companion" topic = "deadlock" - max_action_duration = 5; - min_action_duration = 0.1; + max_action_duration = 3; #=============================================================================== # IMPORTS @@ -72,7 +71,7 @@ def run(self): #Wait for a random amount of time action_duration = random.uniform(0, Constants.max_action_duration) - self.action_client.wait_for_result( rospy.Duration(action_duration) ) + self.action_client.wait_for_result(rospy.Duration(action_duration)) state = self.action_client.get_state() if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING : @@ -92,4 +91,3 @@ def run(self): except: pass - diff --git a/test/test_simple_action_server_deadlock.py b/test/test_simple_action_server_deadlock.py index 364201c2..88c7aea0 100755 --- a/test/test_simple_action_server_deadlock.py +++ b/test/test_simple_action_server_deadlock.py @@ -40,7 +40,7 @@ class Constants: deadlock_timeout = 45 # in seconds shutdown_timeout = 2 # in seconds - max_action_duration = 5; + max_action_duration = 3; #=============================================================================== # IMPORTS @@ -72,7 +72,7 @@ def test_deadlock(self): TestAction, execute_cb = self.execute_callback, auto_start = False) - + self.action_server.register_preempt_callback(self.preempt_callback) self.action_server.start() @@ -103,7 +103,7 @@ def test_deadlock(self): time_since_last_execution = ( termination_time - self.last_execution_time).to_sec() - + self.assertTrue( time_since_last_execution < 2 * Constants.max_action_duration, "Too long since last goal was executed; likely due to a deadlock" ) From 6683eefe081350c234eb9932f8223dd1350cb21c Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 5 Aug 2013 10:48:03 -0700 Subject: [PATCH 3/3] add test depend, remove usage of roslib.load_manifest, pep8 --- package.xml | 2 + ...simple_action_server_deadlock_companion.py | 48 ++++------- test/test_simple_action_server_deadlock.py | 82 ++++++++----------- 3 files changed, 54 insertions(+), 78 deletions(-) diff --git a/package.xml b/package.xml index 2493e793..9ee92f21 100644 --- a/package.xml +++ b/package.xml @@ -31,4 +31,6 @@ rospy rostest std_msgs + + rosnode diff --git a/test/simple_action_server_deadlock_companion.py b/test/simple_action_server_deadlock_companion.py index e2821ead..286b3ab5 100755 --- a/test/simple_action_server_deadlock_companion.py +++ b/test/simple_action_server_deadlock_companion.py @@ -1,6 +1,5 @@ #! /usr/bin/env python # -# # Copyright (c) 2013, Miguel Sarabia # Imperial College London # All rights reserved. @@ -30,57 +29,45 @@ # POSSIBILITY OF SUCH DAMAGE. # -#=============================================================================== -# CONSTANTS -#=============================================================================== + class Constants: - pkg = "actionlib" node = "simple_action_server_deadlock_companion" topic = "deadlock" - - max_action_duration = 3; - -#=============================================================================== -# IMPORTS -#=============================================================================== -import roslib; roslib.load_manifest(Constants.pkg) + max_action_duration = 3 + import random -import rospy + import actionlib from actionlib.msg import TestAction, TestGoal from actionlib_msgs.msg import GoalStatus +import rospy + -#=============================================================================== -# COMPANION CLASS -#=============================================================================== class DeadlockCompanion: - + def __init__(self): - #Seed random with fully resolved name of node and current time - random.seed( rospy.get_name() + str(rospy.Time.now().to_sec()) ) - - #Create actionlib client + # Seed random with fully resolved name of node and current time + random.seed(rospy.get_name() + str(rospy.Time.now().to_sec())) + + # Create actionlib client self.action_client = actionlib.SimpleActionClient( Constants.topic, TestAction) - + def run(self): while not rospy.is_shutdown(): # Send dummy goal - self.action_client.send_goal( TestGoal() ) - - #Wait for a random amount of time + self.action_client.send_goal(TestGoal()) + + # Wait for a random amount of time action_duration = random.uniform(0, Constants.max_action_duration) self.action_client.wait_for_result(rospy.Duration(action_duration)) - + state = self.action_client.get_state() - if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING : + if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING: self.action_client.cancel_goal() -#=============================================================================== -# MAIN -#=============================================================================== if __name__ == '__main__': rospy.init_node(Constants.node) try: @@ -90,4 +77,3 @@ def run(self): raise except: pass - diff --git a/test/test_simple_action_server_deadlock.py b/test/test_simple_action_server_deadlock.py index 88c7aea0..ef0fc8c3 100755 --- a/test/test_simple_action_server_deadlock.py +++ b/test/test_simple_action_server_deadlock.py @@ -1,6 +1,5 @@ #! /usr/bin/env python # -# # Copyright (c) 2013, Miguel Sarabia # Imperial College London # All rights reserved. @@ -30,75 +29,66 @@ # POSSIBILITY OF SUCH DAMAGE. # -#=============================================================================== -# CONSTANTS -#=============================================================================== + class Constants: pkg = "actionlib" node = "test_simple_action_server_deadlock" topic = "deadlock" - - deadlock_timeout = 45 # in seconds - shutdown_timeout = 2 # in seconds - max_action_duration = 3; - -#=============================================================================== -# IMPORTS -#=============================================================================== -import roslib; roslib.load_manifest(Constants.pkg) -import sys + deadlock_timeout = 45 # in seconds + shutdown_timeout = 2 # in seconds + max_action_duration = 3 + import random +import sys import threading import unittest -import rospy -import rosnode + import actionlib from actionlib.msg import TestAction +import rosnode +import rospy -#=============================================================================== -# TEST CLASS -#=============================================================================== class DeadlockTest(unittest.TestCase): def test_deadlock(self): - #Prepare condition (for safe preemption) + # Prepare condition (for safe preemption) self.condition = threading.Condition() self.last_execution_time = None - + # Prepare Simple Action Server self.action_server = actionlib.SimpleActionServer( Constants.topic, TestAction, - execute_cb = self.execute_callback, - auto_start = False) + execute_cb=self.execute_callback, + auto_start=False) self.action_server.register_preempt_callback(self.preempt_callback) self.action_server.start() - #Sleep for the amount specified - rospy.sleep(Constants.deadlock_timeout); - - #Start actual tests + # Sleep for the amount specified + rospy.sleep(Constants.deadlock_timeout) + + # Start actual tests running_nodes = set(rosnode.get_node_names()) required_nodes = { "/deadlock_companion_1", "/deadlock_companion_2", "/deadlock_companion_3", "/deadlock_companion_4", - "/deadlock_companion_5" } - - self.assertTrue( required_nodes.issubset(running_nodes), + "/deadlock_companion_5"} + + self.assertTrue(required_nodes.issubset(running_nodes), "Required companion nodes are not currently running") - + # Shutdown companions so that we can exit nicely termination_time = rospy.Time.now() rosnode.kill_nodes(required_nodes) - + rospy.sleep(Constants.shutdown_timeout) - - #Check last execution wasn't too long ago... - self.assertIsNotNone( self.last_execution_time is None, + + # Check last execution wasn't too long ago... + self.assertIsNotNone(self.last_execution_time is None, "Execute Callback was never executed") time_since_last_execution = ( @@ -106,32 +96,30 @@ def test_deadlock(self): self.assertTrue( time_since_last_execution < 2 * Constants.max_action_duration, - "Too long since last goal was executed; likely due to a deadlock" ) + "Too long since last goal was executed; likely due to a deadlock") def execute_callback(self, goal): - #Note down last_execution time + # Note down last_execution time self.last_execution_time = rospy.Time.now() - - #Determine duration of this action + + # Determine duration of this action action_duration = random.uniform(0, Constants.max_action_duration) - + with self.condition: if not self.action_server.is_preempt_requested(): self.condition.wait(action_duration) - + if self.action_server.is_preempt_requested(): self.action_server.set_preempted() else: self.action_server.set_succeeded() - + def preempt_callback(self): with self.condition: self.condition.notify() -#=============================================================================== -# MAIN -#=============================================================================== + if __name__ == '__main__': import rostest - rospy.init_node( Constants.node ) - rostest.rosrun( Constants.pkg, Constants.node, DeadlockTest ) + rospy.init_node(Constants.node) + rostest.rosrun(Constants.pkg, Constants.node, DeadlockTest)