Beispiel #1
0
 public CommStateMachine(AActionGoal actionGoal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback)
 {
     UnityEngine.Debug.Assert(actionGoal != null);
     this.actionGoal         = actionGoal;
     this.transitionCallback = transitionCallback;
     this.feedbackCallback   = feedbackCallback;
     state = CommState.StateEnum.WAITING_FOR_GOAL_ACK;
 }
Beispiel #2
0
 public void transitionToState(ClientGoalHandle <ActionSpec> gh, CommState next_state)
 {
     ROS.Debug("actionlib", "Trying to transition to %s", next_state.toString());
     setCommState(next_state);
     if (transitionCallback != null)
     {
         transitionCallback(gh);
     }
 }
        void handleTransition(ClientGoalHandle <ActionSpec> gh)
        {
            CommState commState = gh.getCommState();

            switch (commState.state)
            {
            case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
                ROS.Error("actionlib", "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
                break;

            case CommState.StateEnum.PENDING:
                if (goalState != SimpleGoalState.StateEnum.PENDING)
                {
                    ROS.Error("BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
                              commState.toString(), goalState.toString());
                }
                break;

            case CommState.StateEnum.ACTIVE:
                switch (goalState.state)
                {
                case SimpleGoalState.StateEnum.PENDING:
                    setSimpleState(SimpleGoalState.StateEnum.ACTIVE);
                    if (activeCallback != null)
                    {
                        activeCallback();
                    }
                    break;

                case SimpleGoalState.StateEnum.ACTIVE:
                    break;

                case SimpleGoalState.StateEnum.DONE:
                    ROS.Error("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
                              commState.ToString(), goalState.ToString());
                    break;

                default:
                    ROS.Error("Unknown SimpleGoalState %u", goalState.state);
                    return;

                    break;
                }
                break;

            case CommState.StateEnum.WAITING_FOR_RESULT:
                break;

            case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
                break;

            case CommState.StateEnum.RECALLING:
                if (goalState != SimpleGoalState.StateEnum.PENDING)
                {
                    ROS.Error("BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
                              commState.ToString(), goalState.ToString());
                }
                break;

            case CommState.StateEnum.PREEMPTING:
                switch (goalState.state)
                {
                case SimpleGoalState.StateEnum.PENDING:
                    setSimpleState(SimpleGoalState.StateEnum.ACTIVE);
                    if (activeCallback != null)
                    {
                        activeCallback();
                    }
                    break;

                case SimpleGoalState.StateEnum.ACTIVE:
                    break;

                case SimpleGoalState.StateEnum.DONE:
                    ROS.Error("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
                              commState.ToString(), goalState.ToString());
                    break;

                default:
                    ROS.Error("Unknown SimpleGoalState %u", goalState.state);
                    return;

                    break;
                }
                break;

            case CommState.StateEnum.DONE:
                switch (goalState.state)
                {
                case SimpleGoalState.StateEnum.PENDING:
                case SimpleGoalState.StateEnum.ACTIVE:
                    lock ( doneMutex )
                    {
                        setSimpleState(SimpleGoalState.StateEnum.DONE);
                    }

                    if (doneCallback != null)
                    {
                        doneCallback(getState(), gh.getResult());
                    }

                    doneCondition.Set();
//					doneCondition.notify_all();
                    break;

                case SimpleGoalState.StateEnum.DONE:
                    ROS.Error("actionlib", "BUG: Got a second transition to DONE");
                    break;

                default:
                    ROS.Error("Unknown SimpleGoalState %u", goalState.state);
                    return;

                    break;
                }
                break;

            default:
                ROS.Error("actionlib", "Unknown CommState received [%u]", commState.state);
                break;
            }
        }
        /**
         * \brief Get the state information for this goal
         *
         * Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST.
         * \return The goal's state. Returns LOST if this SimpleActionClient isn't tracking a goal.
         */
        SimpleClientGoalState getState()
        {
            if (goalHandle.isExpired())
            {
                ROS.Error("actionlib", "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
                return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.LOST));
            }

            CommState commState = goalHandle.getCommState();

            switch (commState.state)
            {
            case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
            case CommState.StateEnum.PENDING:
            case CommState.StateEnum.RECALLING:
                return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.PENDING));

            case CommState.StateEnum.ACTIVE:
            case CommState.StateEnum.PREEMPTING:
                return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.ACTIVE));

            case CommState.StateEnum.DONE:
            {
                switch (goalHandle.getTerminalState().state)
                {
                case TerminalState.StateEnum.RECALLED:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.RECALLED, goalHandle.getTerminalState().getText()));

                case TerminalState.StateEnum.REJECTED:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.REJECTED, goalHandle.getTerminalState().getText()));

                case TerminalState.StateEnum.PREEMPTED:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.PREEMPTED, goalHandle.getTerminalState().getText()));

                case TerminalState.StateEnum.ABORTED:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.ABORTED, goalHandle.getTerminalState().getText()));

                case TerminalState.StateEnum.SUCCEEDED:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.SUCCEEDED, goalHandle.getTerminalState().getText()));

                case TerminalState.StateEnum.LOST:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.LOST, goalHandle.getTerminalState().getText()));

                default:
                    ROS.Error("actionlib", "Unknown terminal state [%u]. This is a bug in SimpleActionClient", goalHandle.getTerminalState().state);
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.LOST, goalHandle.getTerminalState().getText()));
                }
            }

            case CommState.StateEnum.WAITING_FOR_RESULT:
            case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
                switch (goalState.state)
                {
                case SimpleGoalState.StateEnum.PENDING:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.PENDING));

                case SimpleGoalState.StateEnum.ACTIVE:
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.ACTIVE));

                case SimpleGoalState.StateEnum.DONE:
                    ROS.Error("actionlib", "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
                    return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.LOST));

                default:
                    ROS.Error("actionlib", "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", goalState.state);
                    break;
                }
                break;

            default:
                break;
            }
            ROS.Error("actionlib", "Error trying to interpret CommState - %u", commState.state);
            return(new SimpleClientGoalState(SimpleClientGoalState.StateEnum.LOST));
        }
Beispiel #5
0
        /**
         * \brief Get the terminal state information for this goal
         *
         * Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
         * This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE
         * \return The terminal state
         */
        public TerminalState getTerminalState()
        {
            if (!isActive)
            {
                ROS.Error("actionlib", "Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
                return(new TerminalState(TerminalState.StateEnum.LOST));
            }

            DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
            if (!protector.isProtected())
            {
                ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getTerminalState() call");
                return(new TerminalState(TerminalState.StateEnum.LOST));
            }

            UnityEngine.Debug.Assert(goalManager != null);
            lock ( lockObject )
            {
                CommState comm_state = listHandle.GetElement().getCommState();
//				CommState comm_state = listHandle.getElem()->getCommState();
                if (comm_state != CommState.StateEnum.DONE)
                {
                    ROS.Warn("actionlib", "Asking for the terminal state when we're in [%s]", comm_state.toString());
                }

                Messages.actionlib_msgs.GoalStatus goal_status = listHandle.GetElement().getGoalStatus();
//				Messages.actionlib_msgs.GoalStatus goal_status = listHandle.getElem()->getGoalStatus();

                switch (goal_status.status)
                {
                case gstat.PENDING:
                case gstat.ACTIVE:
                case gstat.PREEMPTING:
                case gstat.RECALLING:
                    ROS.Error("actionlib", "Asking for terminal state, but latest goal status is %u", goal_status.status);
                    return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));

                case gstat.PREEMPTED:
                    return(new TerminalState(TerminalState.StateEnum.PREEMPTED, goal_status.text));

                case gstat.SUCCEEDED:
                    return(new TerminalState(TerminalState.StateEnum.SUCCEEDED, goal_status.text));

                case gstat.ABORTED:
                    return(new TerminalState(TerminalState.StateEnum.ABORTED, goal_status.text));

                case gstat.REJECTED:
                    return(new TerminalState(TerminalState.StateEnum.REJECTED, goal_status.text));

                case gstat.RECALLED:
                    return(new TerminalState(TerminalState.StateEnum.RECALLED, goal_status.text));

                case gstat.LOST:
                    return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));

                default:
                    ROS.Error("actionlib", "Unknown goal status: %u", goal_status.status);
                    break;
                }

                ROS.Error("actionlib", "Bug in determining terminal state");
                return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));
            }
        }
Beispiel #6
0
 void setCommState(CommState state)
 {
     ROS.Debug("actionlib", "Transitioning CommState from %s to %s", state.toString(), state.toString());
     state = state;
 }