public void ProcessLost(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle)
{
ROS.Warn()("Transitioning goal to LOST");
if (goalHandle.LatestGoalStatus != null)
{
goalHandle.LatestGoalStatus.status = GoalStatus.LOST;
goalHandle.LatestGoalStatus.text = "LOST";
}
TransitionToState(goalHandle, CommunicationState.DONE);
}
public ClientGoalHandle <TGoal, TResult, TFeedback> SendGoal(
TGoal goal,
Action <ClientGoalHandle <TGoal, TResult, TFeedback> > OnTransistionCallback = null,
Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > OnFeedbackCallback = null
)
{
// Create Goal Message;
var goalId = new GoalID();
lock (lockId)
{
var now = ROS.GetTime();
// Create sortable goal id
goalId.id = $"{ThisNode.Name}-{nextGoalId:x08}-{now.data.sec:x08}.{now.data.nsec:x08}";
goalId.stamp = now;
nextGoalId = nextGoalId + 1;
}
// Prepare Goal Message
var goalAction = new GoalActionMessage <TGoal>
{
Header = new Messages.std_msgs.Header
{
stamp = ROS.GetTime()
},
GoalId = goalId,
Goal = goal
};
// Register goal message
var goalHandle = new ClientGoalHandle <TGoal, TResult, TFeedback>(
this,
goalAction,
OnTransistionCallback,
OnFeedbackCallback
);
lock (gate)
{
goalHandles[goalAction.GoalId.id] = goalHandle;
}
// Publish goal message
GoalPublisher.Publish(goalAction);
ROS.Debug()("Goal published: {0}", goalHandle.Id);
return(goalHandle);
}
private void UpdateStatus(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle, GoalStatus goalStatus)
{
// Check if ping action is correctly reflected by the status message
if (goalStatus != null)
{
goalHandle.LatestGoalStatus = goalStatus;
}
else
{
if ((goalHandle.State != CommunicationState.WAITING_FOR_GOAL_ACK) &&
(goalHandle.State != CommunicationState.WAITING_FOR_RESULT) &&
(goalHandle.State != CommunicationState.DONE))
{
ProcessLost(goalHandle);
return;
}
else
{
Logger.LogDebug($"goal status is null for {goalHandle.Id}, most propably because it was just send and there" +
$"and the server has not yet sent an update");
return;
}
}
if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK)
{
if (goalStatus.status == GoalStatus.PENDING)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.PENDING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
// NOP
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.ACTIVE)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid transition from ACTIVE to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
// NOP
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
ROS.Error()("Invalid transition from ACTIVE to REJECTED");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid transition from ACTIVE to RECALLING");
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
ROS.Error()("Invalid transition from ACTIVE to RECALLED");
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_RESULT)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from WAITING_FOR_RESUT to PENDING");
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
ROS.Error()("Invalid transition from WAITING_FOR_RESUT to PREEMPTING");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid transition from WAITING_FOR_RESUT to RECALLING");
}
else if (goalStatus.status == GoalStatus.ACTIVE ||
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.REJECTED ||
goalStatus.status == GoalStatus.RECALLED)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK)
{
if (
goalStatus.status == GoalStatus.PENDING ||
goalStatus.status == GoalStatus.ACTIVE
)
{
// NOP
}
else if (
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.PREEMPTED
)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.RECALLING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from RECALLING to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from RECALLING to ACTIVE");
}
else if (
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.PREEMPTED
)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.PREEMPTING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from PREEMPTING to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from PREEMPTING to ACTIVE");
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
ROS.Error()("Invalid Transition from PREEMPTING to REJECTED");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid Transition from PREEMPTING to RECALLING");
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
ROS.Error()("Invalid Transition from PREEMPTING to RECALLED");
}
else if (
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED
)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.DONE)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from DONE to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from DONE to ACTIVE");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid Transition from DONE to RECALLING");
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
ROS.Error()("Invalid Transition from DONE to PREEMPTING");
}
else if (
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.RECALLED ||
goalStatus.status == GoalStatus.REJECTED
)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else
{
ROS.Error()("Invalid comm State: %u", goalHandle.State);
}
}
public void TransitionToState(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle, CommunicationState nextState)
{
ROS.Debug()($"Transitioning CommState from {goalHandle.State} to {nextState}");
goalHandle.FireTransitionCallback(nextState);
}
private void UpdateStatus(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle, GoalStatus goalStatus)
{
// process the status message
if (goalStatus != null)
{
goalHandle.LatestGoalStatus = goalStatus;
goalHandle.statusMissing = 0;
}
else
{
goalHandle.statusMissing += 1;
if (goalHandle.State != CommunicationState.WAITING_FOR_GOAL_ACK &&
goalHandle.State != CommunicationState.WAITING_FOR_RESULT &&
goalHandle.State != CommunicationState.DONE)
{
ProcessLost(goalHandle);
return;
}
else
{
if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK)
{
ROS.Debug()($"Goal status is null for {goalHandle.Id}, most propably because it was just send and there" +
$"and the server has not yet sent an update");
// if goal was not seen for thre status updates republish the goal
if (goalHandle.statusMissing > MAX_STATUS_MISSING_UNACKNOWLEDGED)
{
this.GoalPublisher.Publish(goalHandle.Goal);
goalHandle.statusMissing = 0;
}
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_RESULT)
{
ROS.Debug()($"Goal status is null for {goalHandle.Id}, while waiting for result. Did we miss the result?");
// the server forgot about the goal handle, but we have not received a result message
if (goalHandle.statusMissing > MAX_STATUS_MISSING_WAIT_RESULT)
{
this.ProcessLost(goalHandle);
}
}
return;
}
}
if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK)
{
if (goalStatus.status == GoalStatus.PENDING)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.PENDING);
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.PENDING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
// NOP
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.ACTIVE);
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.ACTIVE)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid transition from ACTIVE to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
// NOP
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
ROS.Error()("Invalid transition from ACTIVE to REJECTED");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid transition from ACTIVE to RECALLING");
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
ROS.Error()("Invalid transition from ACTIVE to RECALLED");
}
else if (goalStatus.status == GoalStatus.PREEMPTED)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_RESULT)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from WAITING_FOR_RESUT to PENDING");
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
ROS.Error()("Invalid transition from WAITING_FOR_RESUT to PREEMPTING");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid transition from WAITING_FOR_RESUT to RECALLING");
}
else if (goalStatus.status == GoalStatus.ACTIVE ||
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.REJECTED ||
goalStatus.status == GoalStatus.RECALLED)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK)
{
if (
goalStatus.status == GoalStatus.PENDING ||
goalStatus.status == GoalStatus.ACTIVE
)
{
// NOP
}
else if (
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.PREEMPTED
)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
TransitionToState(goalHandle, CommunicationState.RECALLING);
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.RECALLING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from RECALLING to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from RECALLING to ACTIVE");
}
else if (
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.PREEMPTED
)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
TransitionToState(goalHandle, CommunicationState.PREEMPTING);
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.PREEMPTING)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from PREEMPTING to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from PREEMPTING to ACTIVE");
}
else if (goalStatus.status == GoalStatus.REJECTED)
{
ROS.Error()("Invalid Transition from PREEMPTING to REJECTED");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid Transition from PREEMPTING to RECALLING");
}
else if (goalStatus.status == GoalStatus.RECALLED)
{
ROS.Error()("Invalid Transition from PREEMPTING to RECALLED");
}
else if (
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED
)
{
TransitionToState(goalHandle, CommunicationState.WAITING_FOR_RESULT);
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown State from the ActionServer. status = %u", goalStatus.status);
}
}
else if (goalHandle.State == CommunicationState.DONE)
{
if (goalStatus.status == GoalStatus.PENDING)
{
ROS.Error()("Invalid Transition from DONE to PENDING");
}
else if (goalStatus.status == GoalStatus.ACTIVE)
{
ROS.Error()("Invalid Transition from DONE to ACTIVE");
}
else if (goalStatus.status == GoalStatus.RECALLING)
{
ROS.Error()("Invalid Transition from DONE to RECALLING");
}
else if (goalStatus.status == GoalStatus.PREEMPTING)
{
ROS.Error()("Invalid Transition from DONE to PREEMPTING");
}
else if (
goalStatus.status == GoalStatus.PREEMPTED ||
goalStatus.status == GoalStatus.SUCCEEDED ||
goalStatus.status == GoalStatus.ABORTED ||
goalStatus.status == GoalStatus.RECALLED ||
goalStatus.status == GoalStatus.REJECTED
)
{
// NOP
}
else
{
ROS.Error()("BUG: Got an unknown status from the ActionServer. status = %u", goalStatus.status);
}
}
else
{
ROS.Error()("Invalid comm State: %u", goalHandle.State);
}
}
internal void FireFeedback(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle, FeedbackActionMessage <TFeedback> feedback)
{
OnFeedbackCallback?.Invoke(goalHandle, feedback);
}
