/**
* \brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
*
* This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
* Note that this does not cancel the goal, it simply stops looking for status info about this goal.
*/
public void stopTrackingGoal()
{
if (goalHandle.isExpired())
{
ROS.Error("actionlib", "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
}
goalHandle = new ClientGoalHandle <ActionSpec> ();
// goalHandle.reset();
}
/**
* \brief Sends a goal to the ActionServer, and also registers callbacks
* \param transitionCallback Callback that gets called on every client state transition
* \param feedbackCallback Callback that gets called whenever feedback for this goal is received
*/
public ClientGoalHandle <ActionSpec> sendGoal <TGoal> (TGoal goal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback) where TGoal : AGoal
{
ROS.Debug("actionlib", "about to start initGoal()");
ClientGoalHandle <ActionSpec> gh = goalManager.initGoal(goal, transitionCallback, feedbackCallback);
ROS.Debug("actionlib", "Done with initGoal()");
return(gh);
}
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);
}
}
public void handleFeedback(ClientGoalHandle <ActionSpec> gh, AFeedback feedback)
{
if (goalHandle != gh)
{
ROS.Error("actionlib", @"Got a callback on a goalHandle that we're not tracking.
This is an internal SimpleActionClient/ActionClient bug.
This could also be a GoalID collision" );
}
if (feedbackCallback != null)
{
feedbackCallback(feedback);
}
}
public void updateFeedback(ClientGoalHandle <ActionSpec> gh, AActionFeedback actionFeedback)
{
if (actionGoal.GoalID.id != actionFeedback.GoalStatus.goal_id.id)
{
return;
}
if (feedbackCallback != null)
{
AFeedback feedback = actionFeedback.Feedback.Clone();
feedbackCallback(gh, feedback);
}
}
/* public void updateFeedback<TFeedback> (ClientGoalHandle<ActionSpec> gh, TFeedback actionFeedback) where TFeedback : AActionFeedback, new()
* {
* // Check if this feedback is for us
* if ( actionGoal.GoalID.id != actionFeedback.GoalStatus.goal_id.id )
* // if ( actionGoal.goal_id.id != actionFeedback.status.goal_id.id )
* return;
*
* if ( feedbackCallback != null )
* {
* // EnclosureDeleter<const ActionFeedback> d(actionFeedback);
* // FeedbackConstPtr feedback(&(actionFeedback->feedback), d);
* AFeedback feedback = actionFeedback.Feedback.Clone ();
* feedbackCallback ( gh, feedback );
* }
* }*/
public void updateResult(ClientGoalHandle <ActionSpec> gh, AActionResult actionResult)
{
// Check if this feedback is for us
if (actionGoal.GoalID.id != actionResult.GoalStatus.goal_id.id)
{
// if ( actionGoal.goal_id.id != actionResult.status.goal_id.id )
return;
}
latest_goal_status_ = actionResult.GoalStatus;
// latest_goal_status_ = actionResult.status;
latestResult = actionResult;
switch (state.state)
{
case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
case CommState.StateEnum.PENDING:
case CommState.StateEnum.ACTIVE:
case CommState.StateEnum.WAITING_FOR_RESULT:
case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
case CommState.StateEnum.RECALLING:
case CommState.StateEnum.PREEMPTING:
{
// A little bit of hackery to call all the right state transitions before processing result
gsarray statusArray = new gsarray();
List <gstat> list = new List <gstat> (statusArray.status_list);
list.Add(actionResult.GoalStatus);
// list.Add ( actionResult.status );
statusArray.status_list = list.ToArray();
updateStatus(gh, statusArray);
transitionToState(gh, CommState.StateEnum.DONE);
break;
}
case CommState.StateEnum.DONE:
ROS.Error("actionlib", "Got a result when we were already in the DONE state");
break;
default:
ROS.Error("actionlib", "In a funny comm state: %u", state.state);
break;
}
}
public void updateResults(AActionResult actionResult)
{
lock ( lockObject )
{
var iter = list.GetIterator();
while (iter.GetElement() != null)
{
ClientGoalHandle <ActionSpec> gh = new ClientGoalHandle <ActionSpec> (this, iter.CreateHandle(), guard);
iter.GetElement().updateResult(gh, actionResult);
iter++;
}
// for ( int i = 0; i < list.Count; i++ )
// foreach ( CommStateMachine<ActionSpec> item in list )
// {
// CommStateMachine<ActionSpec> item = list [ i ];
// ClientGoalHandle<ActionSpec> gh = new ClientGoalHandle<ActionSpec> ( this, item.createHandle (), guard );
// item.updateResult ( gh, actionResult );
// }
}
}
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;
}
}
// Transitions caused by messages
public void updateStatus(ClientGoalHandle <ActionSpec> gh, gsarray statusArray)
{
gstat goal_status = findGoalStatus(statusArray.status_list);
// It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state.
// Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721)
if (state == CommState.StateEnum.DONE)
{
return;
}
if (goal_status != null)
{
latest_goal_status_ = goal_status;
}
else
{
if (state != CommState.StateEnum.WAITING_FOR_GOAL_ACK &&
state != CommState.StateEnum.WAITING_FOR_RESULT &&
state != CommState.StateEnum.DONE)
{
processLost(gh);
}
return;
}
switch (state.state)
{
case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
{
if (goal_status != null)
{
switch (goal_status.status)
{
case gstat.PENDING:
transitionToState(gh, CommState.StateEnum.PENDING);
break;
case gstat.ACTIVE:
transitionToState(gh, CommState.StateEnum.ACTIVE);
break;
case gstat.PREEMPTED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.PREEMPTING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.SUCCEEDED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.ABORTED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.REJECTED:
transitionToState(gh, CommState.StateEnum.PENDING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.RECALLED:
transitionToState(gh, CommState.StateEnum.PENDING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.PREEMPTING:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.PREEMPTING);
break;
case gstat.RECALLING:
transitionToState(gh, CommState.StateEnum.PENDING);
transitionToState(gh, CommState.StateEnum.RECALLING);
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown status from the ActionServer. status = %u", goal_status.status);
break;
}
}
break;
}
case CommState.StateEnum.PENDING:
{
switch (goal_status.status)
{
case gstat.PENDING:
break;
case gstat.ACTIVE:
transitionToState(gh, CommState.StateEnum.ACTIVE);
break;
case gstat.PREEMPTED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.PREEMPTING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.SUCCEEDED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.ABORTED:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.REJECTED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.RECALLED:
transitionToState(gh, CommState.StateEnum.RECALLING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.PREEMPTING:
transitionToState(gh, CommState.StateEnum.ACTIVE);
transitionToState(gh, CommState.StateEnum.PREEMPTING);
break;
case gstat.RECALLING:
transitionToState(gh, CommState.StateEnum.RECALLING);
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.ACTIVE:
{
switch (goal_status.status)
{
case gstat.PENDING:
ROS.Error("actionlib", "Invalid transition from ACTIVE to PENDING"); break;
case gstat.ACTIVE:
break;
case gstat.REJECTED:
ROS.Error("actionlib", "Invalid transition from ACTIVE to REJECTED"); break;
case gstat.RECALLING:
ROS.Error("actionlib", "Invalid transition from ACTIVE to RECALLING"); break;
case gstat.RECALLED:
ROS.Error("actionlib", "Invalid transition from ACTIVE to RECALLED"); break;
case gstat.PREEMPTED:
transitionToState(gh, CommState.StateEnum.PREEMPTING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.SUCCEEDED:
case gstat.ABORTED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT); break;
case gstat.PREEMPTING:
transitionToState(gh, CommState.StateEnum.PREEMPTING); break;
default:
ROS.Error("actionlib", "BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.WAITING_FOR_RESULT:
{
switch (goal_status.status)
{
case gstat.PENDING:
ROS.Error("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PENDING"); break;
case gstat.PREEMPTING:
ROS.Error("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break;
case gstat.RECALLING:
ROS.Error("actionlib", "Invalid Transition from WAITING_FOR_RESUT to RECALLING"); break;
case gstat.ACTIVE:
case gstat.PREEMPTED:
case gstat.SUCCEEDED:
case gstat.ABORTED:
case gstat.REJECTED:
case gstat.RECALLED:
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
{
switch (goal_status.status)
{
case gstat.PENDING:
break;
case gstat.ACTIVE:
break;
case gstat.SUCCEEDED:
case gstat.ABORTED:
case gstat.PREEMPTED:
transitionToState(gh, CommState.StateEnum.PREEMPTING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.RECALLED:
transitionToState(gh, CommState.StateEnum.RECALLING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.REJECTED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT); break;
case gstat.PREEMPTING:
transitionToState(gh, CommState.StateEnum.PREEMPTING); break;
case gstat.RECALLING:
transitionToState(gh, CommState.StateEnum.RECALLING); break;
default:
ROS.Error("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.RECALLING:
{
switch (goal_status.status)
{
case gstat.PENDING:
ROS.Error("actionlib", "Invalid Transition from RECALLING to PENDING"); break;
case gstat.ACTIVE:
ROS.Error("actionlib", "Invalid Transition from RECALLING to ACTIVE"); break;
case gstat.SUCCEEDED:
case gstat.ABORTED:
case gstat.PREEMPTED:
transitionToState(gh, CommState.StateEnum.PREEMPTING);
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.RECALLED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT);
break;
case gstat.REJECTED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT); break;
case gstat.PREEMPTING:
transitionToState(gh, CommState.StateEnum.PREEMPTING); break;
case gstat.RECALLING:
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.PREEMPTING:
{
switch (goal_status.status)
{
case gstat.PENDING:
ROS.Error("actionlib", "Invalid Transition from PREEMPTING to PENDING"); break;
case gstat.ACTIVE:
ROS.Error("actionlib", "Invalid Transition from PREEMPTING to ACTIVE"); break;
case gstat.REJECTED:
ROS.Error("actionlib", "Invalid Transition from PREEMPTING to REJECTED"); break;
case gstat.RECALLING:
ROS.Error("actionlib", "Invalid Transition from PREEMPTING to RECALLING"); break;
case gstat.RECALLED:
ROS.Error("actionlib", "Invalid Transition from PREEMPTING to RECALLED"); break;
break;
case gstat.PREEMPTED:
case gstat.SUCCEEDED:
case gstat.ABORTED:
transitionToState(gh, CommState.StateEnum.WAITING_FOR_RESULT); break;
case gstat.PREEMPTING:
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
case CommState.StateEnum.DONE:
{
switch (goal_status.status)
{
case gstat.PENDING:
ROS.Error("actionlib", "Invalid Transition from DONE to PENDING"); break;
case gstat.ACTIVE:
ROS.Error("actionlib", "Invalid Transition from DONE to ACTIVE"); break;
case gstat.RECALLING:
ROS.Error("actionlib", "Invalid Transition from DONE to RECALLING"); break;
case gstat.PREEMPTING:
ROS.Error("actionlib", "Invalid Transition from DONE to PREEMPTING"); break;
case gstat.PREEMPTED:
case gstat.SUCCEEDED:
case gstat.ABORTED:
case gstat.RECALLED:
case gstat.REJECTED:
break;
default:
ROS.Error("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status.status);
break;
}
break;
}
default:
ROS.Error("actionlib", "In a funny comm state: %u", state.state);
break;
}
}
public void processLost(ClientGoalHandle <ActionSpec> gh)
{
ROS.Warn("actionlib", "Transitioning goal to LOST");
latest_goal_status_.status = gstat.LOST;
transitionToState(gh, CommState.StateEnum.DONE);
}
// Forced transitions
public void transitionToState(ClientGoalHandle <ActionSpec> gh, CommState.StateEnum next_state)
{
transitionToState(gh, new CommState(next_state));
}
