From 5df1483d6dfb58d7a018368a330a70534acf6e2b Mon Sep 17 00:00:00 2001 From: Loy Date: Fri, 30 Oct 2015 22:26:33 +0100 Subject: [PATCH 1/2] SimpleActionState will no longer wait forever for a missing ActionServer --- smach_ros/src/smach_ros/simple_action_state.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/smach_ros/src/smach_ros/simple_action_state.py b/smach_ros/src/smach_ros/simple_action_state.py index 66b8167..0b56820 100644 --- a/smach_ros/src/smach_ros/simple_action_state.py +++ b/smach_ros/src/smach_ros/simple_action_state.py @@ -225,8 +225,9 @@ def _wait_for_server(self): """Internal method for waiting for the action server This is run in a separate thread and allows construction of this state to not block the construction of other states. - """ - while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown(): + """ + timeout_time = rospy.get_rostime() + timeout + while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown() and not rospy.get_rostime() >= timeout_time: try: if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout): self._status = SimpleActionState.INACTIVE From 5225d6071b6a0151df0b09cf550aa3ca581df7c4 Mon Sep 17 00:00:00 2001 From: Loy Date: Fri, 30 Oct 2015 22:32:08 +0100 Subject: [PATCH 2/2] Using correct timeout --- smach_ros/src/smach_ros/simple_action_state.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smach_ros/src/smach_ros/simple_action_state.py b/smach_ros/src/smach_ros/simple_action_state.py index 0b56820..3c7ab82 100644 --- a/smach_ros/src/smach_ros/simple_action_state.py +++ b/smach_ros/src/smach_ros/simple_action_state.py @@ -226,7 +226,7 @@ def _wait_for_server(self): This is run in a separate thread and allows construction of this state to not block the construction of other states. """ - timeout_time = rospy.get_rostime() + timeout + timeout_time = rospy.get_rostime() + self._server_wait_timeout while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown() and not rospy.get_rostime() >= timeout_time: try: if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout):