/* 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; } }