Go to the documentation of this file.00001
00002
00003 """ Provides an execution queue for the roboframenet interface.
00004
00005 Manages incoming callback requests (in the form of filled semantic frames), and calls them in order of request.
00006 """
00007
00008 import roslib; roslib.load_manifest('executor')
00009 import rospy
00010 import actionlib
00011 import actionlib_msgs.msg
00012 import threading
00013 from collections import deque
00014 from roboframenet_msgs.msg import FilledSemanticFrameAction, FilledSemanticFrameGoal, Procedure
00015
00016 MAX_WAIT_TIME = 3.0
00017
00018 client = False
00019 sm_is_running = False
00020 need_to_preempt = False
00021
00022 def cb_procedure(msg):
00023 global client, sm_is_running
00024
00025 need_to_preempt = True
00026 if client:
00027 rospy.loginfo("Requesting goal cancellation.")
00028 client.cancel_all_goals()
00029
00030 r = rospy.Rate(10)
00031 while sm_is_running:
00032 r.sleep()
00033
00034 need_to_preempt = False
00035 t = threading.Thread(target=lambda: execute_sm_wrapper(msg.frame_lists))
00036 t.start()
00037 return
00038
00039
00040 def execute_sm_wrapper(frame_lists):
00041 global sm_is_running
00042 sm_is_running = True
00043 r = execute_sm(frame_lists)
00044 sm_is_running = False
00045
00046 def execute_sm(frame_lists):
00047
00048
00049
00050
00051
00052
00053
00054
00055 global need_to_preempt
00056 rospy.loginfo("Executing frame_lists:\n" + str(frame_lists))
00057 success_overall = False
00058 for frame_list in frame_lists:
00059 rospy.loginfo(" Executing frame sequence: " +
00060 ', '.join(map(lambda frame: frame.name, frame_list.frames)))
00061 success_frame_list = False
00062 for frame in frame_list.frames:
00063 rospy.loginfo(" Executing frame: " + frame.name)
00064 success_frame = False
00065 for topic in frame.action_server_topics:
00066
00067 if need_to_preempt:
00068 rospy.loginfo(" Preempted!.")
00069 return False
00070 rospy.loginfo(" Calling topic: " + topic)
00071 result = call_server(frame, topic)
00072 rospy.loginfo(" Server terminated with status " +
00073 str(result) + " (" +
00074 ("PENDING" if result == 0 else
00075 "ACTIVE" if result == 1 else
00076 "PREEMPTED" if result == 2 else
00077 "SUCCEEDED" if result == 3 else
00078 "ABORTED" if result == 4 else
00079 "REJECTED" if result == 5 else
00080 "PREEMPTING" if result == 6 else
00081 "RECALLING" if result == 7 else
00082 "RECALLED" if result == 8 else
00083 "LOST" if result == 9 else
00084 "unknown status id") +
00085 ").")
00086 if result == actionlib_msgs.msg.GoalStatus.SUCCEEDED:
00087 rospy.loginfo(" ...Success with called topic!")
00088 success_frame = True
00089 break
00090 elif result in [actionlib_msgs.msg.GoalStatus.ABORTED,
00091 actionlib_msgs.msg.GoalStatus.REJECTED,
00092 actionlib_msgs.msg.GoalStatus.RECALLED]:
00093 rospy.logwarn(" ...Failure with called topic.")
00094 break
00095 elif result == actionlib_msgs.msg.GoalStatus.PREEMPTED:
00096 rospy.loginfo(" ...Preempted!.")
00097 return False
00098 else:
00099 rospy.logerr("Unknown actionlib state!: " + str(result) +
00100 " -- Aborting.")
00101 return False
00102
00103 if not success_frame:
00104 rospy.logwarn(" ...Failure with frame.")
00105 break
00106 rospy.loginfo(" ...Success with frame!")
00107
00108 if not success_frame:
00109 success_frame_list = False
00110 rospy.logwarn(" ...Failure with frame sequence.")
00111 else:
00112 success_frame_list = True
00113 success_overall = True
00114 rospy.loginfo(" ...Success with frame sequence!")
00115 break
00116
00117 if not success_overall:
00118 rospy.logerr("...Failure overall.")
00119 else:
00120 rospy.loginfo("...Success overall!")
00121 is_running = False
00122 return success_overall
00123
00124
00125
00126 def call_server(semantic_frame, action_topic):
00127
00128
00129 global client
00130 client = actionlib.SimpleActionClient(action_topic, FilledSemanticFrameAction)
00131 if not client.wait_for_server(rospy.Duration.from_sec(MAX_WAIT_TIME)):
00132 rospy.logerr("call_server: Server with topic " + action_topic +
00133 " has not responded. Aborting.")
00134 return actionlib_msgs.msg.GoalStatus.ABORTED
00135
00136 goal = FilledSemanticFrameGoal()
00137 goal.frame = semantic_frame
00138 client.send_goal(goal)
00139 r = rospy.Rate(10)
00140 s = client.get_state()
00141 while s in [actionlib_msgs.msg.GoalStatus.PENDING,
00142 actionlib_msgs.msg.GoalStatus.ACTIVE,
00143 actionlib_msgs.msg.GoalStatus.PREEMPTING,
00144 actionlib_msgs.msg.GoalStatus.RECALLING,
00145 actionlib_msgs.msg.GoalStatus.LOST]:
00146
00147
00148
00149 r.sleep()
00150 s = client.get_state()
00151 return s
00152
00153
00154 def main():
00155 rospy.init_node('executor')
00156 rospy.Subscriber('procedure', Procedure, cb_procedure)
00157 rospy.spin()
00158
00159
00160
00161 if __name__ == '__main__':
00162 main()
00163
00164
00165