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