/* 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 updateStatuses(gsarray statusArray)
{
lock ( lockObject )
{
var iter = list.GetIterator();
while (iter.GetElement() != null)
{
ClientGoalHandle <ActionSpec> gh = new ClientGoalHandle <ActionSpec> (this, iter.CreateHandle(), guard);
iter.GetElement().updateStatus(gh, statusArray);
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.updateStatus ( gh, statusArray );
// }
}
}
// 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;
}
}
