executor.py

Go to the documentation of this file.

#!/usr/bin/env python
# executor.py
""" Provides an execution queue for the roboframenet interface.

Manages incoming callback requests (in the form of filled semantic frames), and calls them in order of request.
"""

import roslib; roslib.load_manifest('executor')
import rospy
import actionlib
import actionlib_msgs.msg
import threading
from collections import deque
from roboframenet_msgs.msg import FilledSemanticFrameAction, FilledSemanticFrameGoal, Procedure

MAX_WAIT_TIME = 3.0 # seconds

client = False
sm_is_running = False
need_to_preempt = False

def cb_procedure(msg):
    global client, sm_is_running
    # Preempt currently-running
    need_to_preempt = True
    if client:
        rospy.loginfo("Requesting goal cancellation.")
        client.cancel_all_goals()
    # Wait for servers to stop running
    r = rospy.Rate(10) #hz
    while sm_is_running:
        r.sleep()
    # Start our new server.
    need_to_preempt = False
    t = threading.Thread(target=lambda: execute_sm_wrapper(msg.frame_lists))
    t.start()
    return


def execute_sm_wrapper(frame_lists):
    global sm_is_running
    sm_is_running = True
    r = execute_sm(frame_lists)
    sm_is_running = False

def execute_sm(frame_lists):
    # call one-by-one until one succeeds
    # frame_lists is listof(listof(FilledSemanticFrame)), where
    #   listof(FilledSemanticFrame) is a sequence of actions which
    #   need to execute, in order of appearance.  To successfully
    #   complete execution of the sequence, all elements
    #   therein must successfully execute.  If they do not,
    #   the next list will start executing.  (In other words,
    #   inner lists are "and"ed, and outer lists are "or"ed.)
    global need_to_preempt
    rospy.loginfo("Executing frame_lists:\n" + str(frame_lists))
    success_overall = False # We completed our action
    for frame_list in frame_lists:
        rospy.loginfo("  Executing frame sequence: " +
                      ', '.join(map(lambda frame: frame.name, frame_list.frames)))
        success_frame_list = False # We completed our frame list
        for frame in frame_list.frames:
            rospy.loginfo("    Executing frame: " + frame.name)
            success_frame = False
            for topic in frame.action_server_topics:
                # Check that we don't have new goals
                if need_to_preempt:
                    rospy.loginfo("    Preempted!.")
                    return False
                rospy.loginfo("      Calling topic: " + topic)
                result = call_server(frame, topic)
                rospy.loginfo("        Server terminated with status " +
                              str(result) + " (" +
                              ("PENDING"    if result == 0 else
                               "ACTIVE"     if result == 1 else
                               "PREEMPTED"  if result == 2 else
                               "SUCCEEDED"  if result == 3 else
                               "ABORTED"    if result == 4 else
                               "REJECTED"   if result == 5 else
                               "PREEMPTING" if result == 6 else
                               "RECALLING"  if result == 7 else
                               "RECALLED"   if result == 8 else
                               "LOST"       if result == 9 else
                               "unknown status id") +
                              ").")
                if result == actionlib_msgs.msg.GoalStatus.SUCCEEDED:
                    rospy.loginfo("      ...Success with called topic!")
                    success_frame = True
                    break
                elif result in [actionlib_msgs.msg.GoalStatus.ABORTED,
                                actionlib_msgs.msg.GoalStatus.REJECTED,
                                actionlib_msgs.msg.GoalStatus.RECALLED]:
                    rospy.logwarn("      ...Failure with called topic.")
                    break
                elif result == actionlib_msgs.msg.GoalStatus.PREEMPTED:
                    rospy.loginfo("      ...Preempted!.")
                    return False
                else:
                    rospy.logerr("Unknown actionlib state!: " + str(result) +
                                 " -- Aborting.")
                    return False
            # end "for topic"
            if not success_frame:
                rospy.logwarn("    ...Failure with frame.")
                break
            rospy.loginfo("    ...Success with frame!")
        # end "for frame"
        if not success_frame:
            success_frame_list = False # Failed frame -> failed sequence
            rospy.logwarn("  ...Failure with frame sequence.")
        else:
            success_frame_list = True
            success_overall = True
            rospy.loginfo("  ...Success with frame sequence!")
            break
    # end "for frame_list"
    if not success_overall:
        rospy.logerr("...Failure overall.")
    else:
        rospy.loginfo("...Success overall!")
    is_running = False
    return success_overall


# We will be in a terminal state upon exit.
def call_server(semantic_frame, action_topic):
    # rospy.loginfo("Calling topic '" + action_topic +
    #              "'.  Frame: " + str(semantic_frame.name))
    global client
    client = actionlib.SimpleActionClient(action_topic, FilledSemanticFrameAction)
    if not client.wait_for_server(rospy.Duration.from_sec(MAX_WAIT_TIME)):
        rospy.logerr("call_server: Server with topic " + action_topic +
                     " has not responded.  Aborting.")
        return actionlib_msgs.msg.GoalStatus.ABORTED

    goal = FilledSemanticFrameGoal()
    goal.frame = semantic_frame
    client.send_goal(goal)
    r = rospy.Rate(10) #hz
    s = client.get_state()
    while s in [actionlib_msgs.msg.GoalStatus.PENDING,
                actionlib_msgs.msg.GoalStatus.ACTIVE,
                actionlib_msgs.msg.GoalStatus.PREEMPTING,
                actionlib_msgs.msg.GoalStatus.RECALLING,
                actionlib_msgs.msg.GoalStatus.LOST]:
        # result should not be actionlib_msgs.msg.GoalStatus.LOST,
        #   as only client should send this to server.
        # Wait for status to become terminal.
        r.sleep()
        s = client.get_state()
    return s


def main():
    rospy.init_node('executor')
    rospy.Subscriber('procedure', Procedure, cb_procedure)
    rospy.spin()



if __name__ == '__main__':
    main()