Attaching FlexBE GUI to a running behavior sometimes throws an error

[manox]

I would like to run a FlexBE behavior directly on a robot (so that it theoretically can run without operator), but want also to attach the FlexBE GUI to it on another PC.

In the specific case, I start the behavior_onboard.launch on the robot and start the behavior with a be_launcher node. Then, Iwant to start the state machine manually after attaching the GUI (flexbe_ocs.launch) to the running behavior. I wonder what the best method is. At the moment, I use a wait state (with very long wait time) as first state, which gives me time to attach the GUI and then fore the outcome of that state. Maybe there are better methods like a state waiting for a start message. The problem is, that sometimes this works and sometimes it's throwing an error like follows:

[ERROR] [1548852953.881947]: Behavior execution failed!
    Could not execute state 'Wait before start' of type '<flexbe_states.wait_state.WaitState object at 0x7fc9a4820e50>': Traceback (most recent call last):
      File "/opt/ros/kinetic/lib/python2.7/dist-packages/smach/state_machine.py", line 247, in _update_once
        self._remappings[self._current_label]))
      File "/home/mark/catkin_ws/src/flexbe_behavior_engine/flexbe_core/src/flexbe_core/core/event_state.py", line 38, in _event_execute
        if self._is_controlled and self._sub.has_msg(self._pause_topic):
      File "/home/mark/catkin_ws/src/flexbe_behavior_engine/flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py", line 153, in has_msg
        return ProxySubscriberCached._topics[topic]['last_msg'] is not None
    KeyError: 'flexbe/command/pause'

Another question that a have regarding FlexBE: Is there is a possibility to automate the process of loading a behavior in the GUI and attaching to a running behavior?

I already postet this question on ROS answers some days ago: https://answers.ros.org/question/314229/attaching-flexbe-gui-to-a-running-behavior-sometimes-throws-an-error/

[pschillinger]

An alternative you can use for waiting: There is an OperatorDecisionState, which does nothing but wait until the operator forces an outcome (if you don't set a suggestion). So you can put this state at the beginning and confirm that the GUI is attached by forcing a continue outcome.

Not sure regarding this error, though. I never saw this when attaching a behavior, but will try to replicate it.

There is no way to load a behavior after GUI launch, yet. I am happy to merge a PR regarding this, but unfortunately, I cannot implement it myself right now.

[manox]

Thanks for the hint to OperatorDecisionState. But while attaching the GUI I sometimes get the same error as before. Nevertheless one can click start then and the GUI is attached.

[joao-aguizo]

I have recently faced this issue myself, are there any updates regarding this?

[pschillinger]

I was still not able to reproduce this issue. "Sometimes" sounds like this occurs non-deterministically, do you have any suggestions how to make it more likely to occur? In which situations did you observe it?

[joao-aguizo]

I have not seen this error for a long time, unfortunately at the time I did not invest that much time in trying to properly debug it, instead I stopped using some state that was problematic and somehow was triggering this error, thus "fixing" the issue, it was not the WaitState though.

The more I look at our maintained states the more I remember about this problem. I believe that at the time I have came across this issue post and read @pschillinger's suggestion about using the OperatorDecisionState instead of the WaitState, then after reading @manox response:

Thanks for the hint to OperatorDecisionState. But while attaching the GUI I sometimes get the same error as before. Nevertheless one can click start then and the GUI is attached.

I have realized that I was using a custom variant of the OperatorDecisionState, called TerminalOperatorDecisionState, in which the idea was similar to the original one but also to allow the user to select one of the listed possible transitions through the CLI. This state relies on a custom action server, the OperatorInputActionServer, which used the original flexbe's BehaviorInputAction defintion. After realizing such thing I stopped using the aforementioned state, thus working around the issue.

operator_input_action_server.py

#! /usr/bin/env python

import rospy
import actionlib
import pickle

from flexbe_msgs.msg import BehaviorInputAction, BehaviorInputResult

class OperatorInputActionServer(object):
    _result = BehaviorInputResult()

    def __init__(self):
        self._as = actionlib.SimpleActionServer('flexbe/operator_input', BehaviorInputAction, execute_cb = self.execute_cb, auto_start = False)
        self._as.start()

        rospy.loginfo('Ready for operator input!')
        
    '''
    Request types:

    ***Any***   >>>   type 'cancel' or any variation to cancel process
    
    0 - Custom question     >>>     [CUSTOM QUESTION] (answer analysis in code)
    1 - Y/N question       >>>     Are you sure you want to '{}'? [Y/n]
    2 - Operator decision state via terminal    >>>     Data contains the possible outcomes, displays and allows the user to choose (0, 1, ..., n)
    '''  
    def execute_cb(self, goal):
        data = ""
        question = ""
        result_code = 0
        outcomes = []
        
        rospy.loginfo("\n\n\n\t OPERATOR INPUT REQUESTED \n\t NOTE: type 'cancel' to abort current behavior \n\n")

        # Question definition (based on predefined types)
        if goal.request_type == 1:
            question = "Are you sure you want to {} [Y/n]? ".format(goal.msg)
        elif goal.request_type == 2:
            question = "Terminal operator decision state reached!" + "\n" + "Possible outcomes:" + "\n"

            outcomes = pickle.loads(goal.msg)
            for idx, out in enumerate(outcomes):
                question = question + "{0} - {1} \n".format(idx, out)

            question = question + "Select index and press [ENTER]: "

        elif goal.request_type == 0:
            question = goal.msg

        # Gathers the user input
        input_data = raw_input(question)
        data = input_data   # this works for the default case of request_type == 0

        # Answer analysis 
        if input_data.lower() == "cancel":
            data = "Canceled upon user request."
            result_code = 2
        else:
            # Analysis for the predefined request types
            if goal.request_type == 1:
                if input_data.lower() in ["", "y", "yes"]:    # pressed ENTER or chosen "yes" (default choice)
                    data = "yes"
                elif input_data.lower() in ["n", "no"]:
                    data = "no"
                else:
                    data = "Invalid option!"
                    result_code = 1

            elif goal.request_type == 2:
                try:
                    idx = int(input_data)
                    if idx > len(outcomes) - 1 or idx < 0:
                        data = "{} is not a valid index".format(idx)
                        result_code = 1
                    else:
                        data = input_data
                except ValueError:
                    data = "Invalid option! Please insert an integer."
                    result_code = 1


        # The pickle dumps is mandatory, since the input state executes pickle.loads()
        self._result.result_code = result_code
        self._result.data = pickle.dumps(data)
        self._as.set_succeeded(self._result)
        

if __name__ == '__main__':
    rospy.init_node('operator_input_action_server')
    server = OperatorInputActionServer()
    rospy.spin()

terminal_operator_decision_state.py

#!/usr/bin/env python
import rospy
import pickle
from actionlib_msgs.msg import GoalStatus
from flexbe_core.proxy import ProxyActionClient

from flexbe_core import EventState, Logger
from flexbe_msgs.msg import BehaviorInputAction, BehaviorInputGoal

'''
Created on 30.03.2021

@author: Joao Aguizo
'''

class TerminalOperatorDecisionState(EventState):
    '''
    Implements a state where the operator has to manually choose an outcome.
    Autonomy Level of all outcomes should be set to Full, because this state is not able to choose an outcome on its own.
    Only exception is the suggested outcome, which will be returned immediately by default.
    This state can be used to create alternative execution paths by setting the suggestion to High autonomy instead of Full.
    Additionaly it requests an input in the terminal, relying on the OperatorInputActionServer from flexbe_widget_additions package.

    -- outcomes     string[]    A list of all possible outcomes of this state.
    -- hint         string      Text displayed to the operator to give instructions how to decide.
    -- suggestion   string      The outcome which is suggested. Will be returned if the level of autonomy is high enough.

    '''

    _goal = BehaviorInputGoal()
    _default_outcomes=["failed", "unreached", "aborted", "invalid", "preempted", "loopback"]
    def __init__(self, outcomes=["failed", "unreached", "aborted", "invalid"], hint=None, suggestion=None):
        '''
        Constructor
        '''
        super(TerminalOperatorDecisionState, self).__init__(outcomes=outcomes)
                
        self._hint = hint
        self._suggestion = suggestion
        self._my_outcomes = outcomes

        # Gathers the user defined outcomes
        self._user_defined_outcomes = []
        for outcome in self._my_outcomes:
            if outcome not in self._default_outcomes:
                self._user_defined_outcomes.append(outcome)

        # Creates the client to request a terminal input
        self._action_topic = "flexbe/operator_input"
        self._client = ProxyActionClient({self._action_topic: BehaviorInputAction})

        # Client flags
        self._unreached = False
        
        
    def execute(self, userdata):
        '''
        Execute this state
        '''
        if self._suggestion is not None and self._my_outcomes.count(self._suggestion) > 0:
            return self._suggestion

        if self._unreached:
            Logger.logwarn('Could not reach operator input action server...')
            return "unreached"
        
        if self._client.has_result(self._action_topic):
            
            status = self._client.get_state(self._action_topic)
            if status == GoalStatus.SUCCEEDED:

                # Checks the result
                result = self._client.get_result(self._action_topic)
                if result.result_code == 0:
                    return self._user_defined_outcomes[int(pickle.loads(result.data))]
                
                elif result.result_code == 1:
                    return "invalid"
                
                elif result.result_code == 2:
                    return "aborted"
                    

            elif status in [GoalStatus.ABORTED, GoalStatus.PREEMPTED, GoalStatus.REJECTED,
                            GoalStatus.RECALLED]:
                Logger.logwarn('Operator input failed: %s' % str(status))
                return "failed"
            

    def on_enter(self, userdata):
        if self._hint is not None:
            Logger.loghint(self._hint)

        # Resets the flag
        self._unreached = False

        # Populates the goal
        self._goal.request_type = 2
        self._goal.msg = pickle.dumps(self._user_defined_outcomes)
        
        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, self._goal)
        except Exception as e:
            Logger.logwarn("Unable to send operator input action goal:\n%s" % str(e))
            self._unreached = True


    def cancel_active_goals(self):
        if self._client.is_available(self._action_topic):
            if self._client.is_active(self._action_topic):
                if not self._client.has_result(self._action_topic):
                    self._client.cancel(self._action_topic)
                    Logger.loginfo('Cancelled operator input active action goal.')


    def on_exit(self, userdata):
        self.cancel_active_goals()


    def on_stop(self):
        self.cancel_active_goals()

The error I would get while running this version of the flexbe_behavior_engine, would be a KeyError: 'flexbe/command/pause'.

With all this I hope you can replicate, let me know if you need any more information.

At a first glance, I believe the following question should be asked:

• Is there any critical common points between the logic flow of the WaitState, OperatorDecisionState and TerminalOperatorDecisionState (and eventually the classes that these derive from)?

As far as I can see, the only similarity between these seems to be that they all either wait for a given period of time, or indefinitely until a transition is requested. Can this particularity eventually be a problem to the their base classes?