Go to the documentation of this file.00001
00002 import sys
00003
00004 import actionlib
00005 import rospy
00006 """
00007 @package manage_actionlib
00008 This module provides utility methods for setting up actionlib calls through an overarching actionlib server.
00009 """
00010
00011
00012 def monitor_action_goal(action_server,
00013 action_client,
00014 action_goal,
00015 action_name="current action",
00016 time_limit=float("inf"),
00017 abort_on_fail=False):
00018 """Send and monitor an action goal to a given action client.
00019
00020 The monitor will return in case of the client reporting success, preemption or
00021 abortion, and will also pass through any incoming preemptions to the action server.
00022
00023 @param action_server Action server that is calling the action client. If the action server is preempted, this is passed to the action client.
00024 @param action_client The action client that is called by the action server. If the action server is preempted, the goal sent to this client is preempted as well.
00025 @param action_goal The action goal to be sent to the action client.
00026 @param action_name Optional name for the action. Will be displayed in the logs.
00027 @param time_limit An optional time limit for the action. If exceeded, the action client is preempted
00028 """
00029
00030 success = False
00031 rospy.loginfo("Sending goal to " + action_name)
00032 action_client.send_goal(action_goal)
00033 init_time = rospy.Time.now()
00034 while action_server.is_active():
00035 if (rospy.Time.now() - init_time).to_sec() > time_limit:
00036 rospy.logwarn("Timeout of request")
00037 action_client.cancel_goal()
00038 success = False
00039 break
00040
00041 if action_server.is_preempt_requested():
00042 rospy.logwarn("Preempting " + action_name)
00043 action_client.cancel_goal()
00044 finished = action_client.wait_for_result(
00045 timeout=rospy.Duration(1.0))
00046
00047 if not finished:
00048 rospy.logerr(action_name +
00049 " failed to preempt! This should never happen")
00050 action_server.set_aborted(text="Aborted due to action " +
00051 action_name + " failing to preempt")
00052 else:
00053 action_server.set_preempted(
00054 text="Preempted while running " + action_name)
00055 rospy.loginfo("Preempted while running")
00056 break
00057
00058 if action_client.get_state() == actionlib.GoalStatus.ABORTED:
00059 rospy.logerr(action_name + " aborted!")
00060 success = False
00061 if abort_on_fail:
00062 action_server.set_aborted(text=action_name + " aborted")
00063 break
00064
00065 if action_client.get_state() == actionlib.GoalStatus.PREEMPTED:
00066 rospy.logerr(action_name + " preempted!")
00067 success = False
00068 if abort_on_fail:
00069 action_server.set_aborted(text=action_name + " was preempted")
00070 break
00071
00072 if action_client.get_state() == actionlib.GoalStatus.SUCCEEDED:
00073 rospy.loginfo(action_name + " succeeded!")
00074 success = True
00075 break
00076
00077 rospy.sleep(0.1)
00078
00079 return success