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;
}
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));
}
/**
* \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));
}
}
void setCommState(CommState state)
{
ROS.Debug("actionlib", "Transitioning CommState from %s to %s", state.toString(), state.toString());
state = state;
}
