/**
* \brief Resends this goal [with the same GoalID] to the ActionServer
*
* Useful if the user thinks that the goal may have gotten lost in transit
*/
public void resend()
{
if (!isActive)
{
ROS.Error("actionlib", "Trying to resend() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
}
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this resend() call");
return;
}
UnityEngine.Debug.Assert(goalManager != null);
AActionGoal actionGoal = null;
lock ( lockObject )
{
actionGoal = listHandle.GetElement().getActionGoal();
// actionGoal = listHandle.getElem()->getActionGoal();
}
if (actionGoal == null)
{
ROS.Error("actionlib", "BUG: Got a NULL actionGoal");
}
if (goalManager.sendGoalDelegate != null)
{
goalManager.sendGoalDelegate(actionGoal);
}
}
public void Cancel()
{
if (!Active)
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] Trying to cancel() on an inactive goal handle.");
}
if ((State == CommunicationState.WAITING_FOR_RESULT ||
State == CommunicationState.RECALLING ||
State == CommunicationState.PREEMPTING ||
State == CommunicationState.DONE))
{
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Got a cancel() request while in state {State}, so ignoring it");
return;
}
else if (!(State == CommunicationState.WAITING_FOR_GOAL_ACK ||
State == CommunicationState.PENDING ||
State == CommunicationState.ACTIVE ||
State == CommunicationState.WAITING_FOR_CANCEL_ACK))
{
ROS.Debug()($"[{ThisNode.Name}] [actionlib] BUG: Unhandled CommState: {State}");
return;
}
var cancelMessage = new GoalID();
cancelMessage.id = Id;
actionClient.CancelPublisher.publish(cancelMessage);
actionClient.TransitionToState(this, CommunicationState.WAITING_FOR_CANCEL_ACK);
CheckDoneAsync();
}
private (int, int) TestStartStopOfRos(int numberOfTestRuns)
{
int succesfulRuns = 0;
int failedRuns = 0;
for (int i = 0; i < numberOfTestRuns; i++)
{
StartRos(i);
try
{
WaitForLatchedMessage(TimeSpan.FromSeconds(10));
succesfulRuns += 1;
}
catch (TimeoutException te)
{
ROS.Error()($"Run #{i} timed out!");
failedRuns += 1;
}
finally
{
StopRos(i);
}
}
return(succesfulRuns, failedRuns);
}
/**
* \brief Store a GoalStatus in a ClientGoalStatus
* Note that the only GoalStatuses that can be converted into a
* ClientGoalStatus are {PREEMPTED, SUCCEEDED, ABORTED, REJECTED}
* \param goal_status The GoalStatus msg that we want to convert
*/
public void fromGoalStatus(GoalStatus goal_status)
{
switch (goal_status.status)
{
case GoalStatus.PREEMPTED:
state_ = StateEnum.PREEMPTED;
break;
case GoalStatus.SUCCEEDED:
state_ = StateEnum.SUCCEEDED;
break;
case GoalStatus.ABORTED:
state_ = StateEnum.ABORTED;
break;
case GoalStatus.REJECTED:
state_ = StateEnum.REJECTED;
break;
default:
state_ = StateEnum.LOST;
ROS.Error("actionlib", "Cannot convert GoalStatus %u to ClientGoalState", goal_status.status);
break;
}
}
public void Resend()
{
if (!this.Active)
{
ROS.Error()("actionlib", "Trying to resend() on an inactive goal handle.");
}
actionClient.GoalPublisher.Publish(Goal);
}
public void Resend()
{
if (!Active)
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] Trying to resend() on an inactive goal handle.");
}
actionClient.GoalPublisher.publish(Goal);
}
/**
* \brief Cancel the goal that we are currently pursuing
*/
public void cancelGoal()
{
if (goalHandle.isExpired())
{
ROS.Error("actionlib", "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
}
goalHandle.cancel();
}
/**
* \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();
}
public bool lookupTransform(string target_frame, string source_frame, Time time, out Transform transform)
{
bool result = lookupTransform(target_frame, source_frame, time, out transform, out string error_string);
if (!result && error_string != null)
{
ROS.Error()(error_string);
}
return(result);
}
public bool getButton(Joy joy, SButton button)
{
if (button.button <= 0 || button.button > joy.buttons.Length)
{
ROS.Error("Button " + button.button + " out of range, joy has " + joy.buttons.Length + " buttons");
return(false);
}
return(joy.buttons[button.button - 1] > 0);
}
/**
* \brief Sends a cancel message for this specific goal to the ActionServer
*
* Also transitions the Communication State Machine to WAITING_FOR_CANCEL_ACK
*/
public void cancel()
{
if (!isActive)
{
ROS.Error("actionlib", "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
return;
}
UnityEngine.Debug.Assert(goalManager != null);
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this call");
return;
}
lock ( lockObject )
{
switch (listHandle.GetElement().getCommState().state)
{
case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
case CommState.StateEnum.PENDING:
case CommState.StateEnum.ACTIVE:
case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
break; // Continue standard processing
case CommState.StateEnum.WAITING_FOR_RESULT:
case CommState.StateEnum.RECALLING:
case CommState.StateEnum.PREEMPTING:
case CommState.StateEnum.DONE:
ROS.Debug("actionlib", "Got a cancel() request while in state [%s], so ignoring it", listHandle.GetElement().getCommState().toString());
return;
default:
ROS.Error("actionlib", "BUG: Unhandled CommState: %u", listHandle.GetElement().getCommState().state);
return;
}
}
AActionGoal actionGoal = listHandle.GetElement().getActionGoal();
Messages.actionlib_msgs.GoalID cancelMsg = new Messages.actionlib_msgs.GoalID();
cancelMsg.stamp = ROS.GetTime();
cancelMsg.id = listHandle.GetElement().getActionGoal().GoalID.id;
// cancelMsg.id = listHandle.GetElement ().getActionGoal ()->goal_id.id;
if (goalManager.cancelDelegate != null)
{
goalManager.cancelDelegate(cancelMsg);
}
listHandle.GetElement().transitionToState(this, CommState.StateEnum.WAITING_FOR_CANCEL_ACK);
}
public void SetRejected(TResult result, string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Setting status to rejected on goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if ((GoalStatus.status == GoalStatus.PENDING) || (GoalStatus.status == GoalStatus.RECALLING))
{
SetGoalResult(GoalStatus.REJECTED, text, result);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: {GoalStatus.status}");
}
}
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 SetAborted(TResult result, string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Setting status to aborted on goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if ((GoalStatus.status == GoalStatus.PREEMPTING) || (GoalStatus.status == GoalStatus.ACTIVE))
{
SetGoalResult(GoalStatus.ABORTED, text, result);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: {GoalStatus.status}");
}
}
/**
* \brief Get the Result of the current goal
* \return shared pointer to the result. Note that this pointer will NEVER be NULL
*/
public AResult getResult()
{
if (goalHandle.isExpired())
{
ROS.Error("actionlib", "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
}
if (goalHandle.getResult() != null)
{
return(goalHandle.getResult());
}
return(null);
}
public void SetSucceded(TResult result, string text)
{
text = text ?? "";
ROS.Debug()("actionlib", $"Setting status to succeeded on goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if ((GoalStatus.status == GoalStatus.PREEMPTING) || (GoalStatus.status == GoalStatus.ACTIVE))
{
SetGoalResult(GoalStatus.SUCCEEDED, text, result);
}
else
{
ROS.Error()("actionlib", "To transition to a succeeded state, the goal must be in a preempting or active state, " +
$"it is currently in state: {GoalStatus.status}");
}
}
/**
* \brief Blocks until this goal finishes
* \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
* \return True if the goal finished. False if the goal didn't finish within the allocated timeout
*/
public bool waitForResult(Duration timeout)
{
if (goalHandle.isExpired())
{
ROS.Error("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
return(false);
}
if (timeout < new Duration(new TimeData(0, 0)))
{
ROS.Warn("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
}
Time timeout_time = ROS.GetTime() + timeout;
lock ( doneMutex )
{
// Hardcode how often we check for node.ok()
Duration loop_period = new Duration().fromSec(.1);
while (nodeHandle.ok)
{
// Determine how long we should wait
Duration time_left = timeout_time - ROS.GetTime();
// Check if we're past the timeout time
if (timeout > new Duration() && time_left <= new Duration())
{
break;
}
if (goalState == SimpleGoalState.StateEnum.DONE)
{
break;
}
// Truncate the time left
if (time_left > loop_period || timeout == new Duration())
{
time_left = loop_period;
}
}
// doneCondition.timed_wait(lock, boost.posix_time.milliseconds(time_left.toSec() * 1000.0f));
}
return(goalState == SimpleGoalState.StateEnum.DONE);
}
public void cb(Stopwatch s)
{
if (active)
{
active = false;
if (callback != null)
{
callback(s);
}
else
{
ROS.Error("actionlib", "Got a NULL Timer OneShotTimer Callback");
}
}
}
private void OnResultMessage(ResultActionMessage <TResult> result)
{
string goalId = result.GoalStatus.goal_id.id;
ROS.Debug()("OnResultMessage (goal_id: {0}, status: {1})", goalId, result.GoalStatus.status);
ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle;
bool goalExists;
lock (gate)
{
goalExists = goalHandles.TryGetValue(result.GoalStatus.goal_id.id, out goalHandle);
}
if (goalExists)
{
ROS.Debug()("Processing result for known goal handle. (goal_id: {0})", goalId);
goalHandle.LatestGoalStatus = result.GoalStatus;
goalHandle.LatestResultAction = result;
if (goalHandle.State == CommunicationState.DONE)
{
ROS.Error()("Got a result when we were already in the DONE state (goal_id: {0})", goalId);
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK ||
goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK ||
goalHandle.State == CommunicationState.PENDING ||
goalHandle.State == CommunicationState.ACTIVE ||
goalHandle.State == CommunicationState.WAITING_FOR_RESULT ||
goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK ||
goalHandle.State == CommunicationState.RECALLING ||
goalHandle.State == CommunicationState.PREEMPTING)
{
UpdateStatus(goalHandle, result.GoalStatus);
TransitionToState(goalHandle, CommunicationState.DONE);
}
else
{
ROS.Error()($"Invalid comm for result message state: {goalHandle.State}.");
}
}
else
{
ROS.Debug()("Ignoring result for unknown goal (goal_id: {0})", goalId);
}
}
public void SetAccepted(string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Accepting goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if (GoalStatus.status == GoalStatus.PENDING)
{
SetGoalStatus(GoalStatus.ACTIVE, text);
}
else if (GoalStatus.status == GoalStatus.RECALLING)
{
SetGoalStatus(GoalStatus.PREEMPTING, text);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: {GoalStatus.status}");
}
}
public void listElemDeleter(List <CommStateMachine <ActionSpec> > .Enumerator it)
{
UnityEngine.Debug.Assert(guard != null);
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal");
return;
}
ROS.Debug("actionlib", "About to erase CommStateMachine");
lock ( lockObject )
{
list.Remove(it.Current);
}
ROS.Debug("actionlib", "Done erasing CommStateMachine");
}
public void SetCanceled(TResult result, string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] Setting status to canceled on goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if ((GoalStatus.status == GoalStatus.PENDING) || (GoalStatus.status == GoalStatus.RECALLING))
{
SetGoalResult(GoalStatus.RECALLED, text, result);
}
else if ((GoalStatus.status == GoalStatus.ACTIVE) || (GoalStatus.status == GoalStatus.PREEMPTING))
{
SetGoalResult(GoalStatus.PREEMPTED, text, result);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: {GoalStatus.status}");
}
}
public double getAxis(Joy joy, SAxis axis)
{
if (axis.axis == 0 || Math.Abs(axis.axis) > joy.axes.Length)
{
ROS.Error("Axis " + axis.axis + " out of range, joy has " + joy.axes.Length + " axes");
return(0);
}
double output = Math.Abs(axis.axis) / axis.axis * joy.axes[Math.Abs(axis.axis) - 1] * axis.factor + axis.offset;
// TODO keep or remove deadzone? may not be needed
// if (Math.Abs(output) < axis.max_ * 0.2)
// {
// output = 0.0;
// }
return(output);
}
public bool waitForActionServerToStart(duration timeout, NodeHandle nh)
{
if (timeout < new duration())
{
ROS.Error("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.data.sec.ToString() + ":" + timeout.data.nsec.ToString());
}
Messages.std_msgs.Time timeout_time = ROS.GetTime() + timeout;
lock ( lockObject )
{
if (isServerConnected())
{
return(true);
}
// Hardcode how often we check for node.ok()
duration loop_period = new duration(new Messages.TimeData(0, 500000000)); // ".fromSec(.5)"
while (nh.ok && !isServerConnected())
{
// Determine how long we should wait
duration time_left = timeout_time - ROS.GetTime();
// Check if we're past the timeout time
if (timeout != new duration(new Messages.TimeData(0, 0)) && time_left <= new duration(new Messages.TimeData(0, 0)))
{
break;
}
// Truncate the time left
if (time_left > loop_period || timeout == new duration())
{
time_left = loop_period;
}
uint msWait = time_left.data.sec * 1000 + time_left.data.nsec / 1000000;
Monitor.Wait(lockObject, UnityEngine.Mathf.Max((int)msWait, 0));
}
return(isServerConnected());
}
}
/**
* \brief Stops goal handle from tracking a goal
*
* Useful if you want to stop tracking the progress of a goal, but it is inconvenient to force
* the goal handle to go out of scope. Has pretty much the same semantics as boost::shared_ptr::reset()
*/
public void reset()
{
if (isActive)
{
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this reset() call");
return;
}
lock ( lockObject )
{
// listHandle.reset(); // not sure what to replace this stuff with
isActive = false;
goalManager = null;
}
}
}
/* 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;
}
}
/**
* \brief Check if two goal handles point to the same goal
* \return TRUE if both point to the same goal. Also returns TRUE if both handles are inactive.
*/
public static bool operator ==(ClientGoalHandle <ActionSpec> lhs, ClientGoalHandle <ActionSpec> rhs)
{
if (object.ReferenceEquals(lhs, rhs))
{
return(true);
}
if (!(lhs.isActive && rhs.isActive))
{
return(false);
}
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(lhs.guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this operator==() call");
return(false);
}
return(lhs.listHandle == rhs.listHandle);
}
private void OnResultMessage(ResultActionMessage <TResult> result)
{
ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle;
bool goalExists;
lock (lockGoalHandles)
{
goalExists = goalHandles.TryGetValue(result.GoalStatus.goal_id.id, out goalHandle);
}
if (goalExists)
{
goalHandle.LatestGoalStatus = result.GoalStatus;
goalHandle.LatestResultAction = result;
if (goalHandle.State == CommunicationState.DONE)
{
ROS.Error()(
"Got a result when we were already in the DONE state (goal_id:" +
$" {result.GoalStatus.goal_id.id})"
);
}
else if ((goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK) ||
(goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK) ||
(goalHandle.State == CommunicationState.PENDING) ||
(goalHandle.State == CommunicationState.ACTIVE) ||
(goalHandle.State == CommunicationState.WAITING_FOR_RESULT) ||
(goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK) ||
(goalHandle.State == CommunicationState.RECALLING) ||
(goalHandle.State == CommunicationState.PREEMPTING))
{
UpdateStatus(goalHandle, result.GoalStatus);
TransitionToState(goalHandle, CommunicationState.DONE);
}
else
{
ROS.Error()($"Invalid comm for result message state: {goalHandle.State}.");
}
}
}
/**
* \brief Get the state of this goal's communication state machine from interaction with the server
*
* Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT,
* WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE
* \return The current goal's communication state with the server
*/
public CommState getCommState()
{
if (!isActive)
{
ROS.Error("actionlib", "Trying to getCommState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
return(new CommState(CommState.StateEnum.DONE));
}
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getCommState() call");
return(new CommState(CommState.StateEnum.DONE));
}
UnityEngine.Debug.Assert(goalManager != null);
lock ( lockObject )
{
return(listHandle.GetElement().getCommState());
// return listHandle.getElem()->getCommState(); // ??
}
}
/**
* \brief Get result associated with this goal
*
* \return NULL if no reseult received. Otherwise returns shared_ptr to result.
*/
public AResult getResult()
// public AActionResult getResult ()
{
if (!isActive)
{
ROS.Error("actionlib", "Trying to getResult on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
}
UnityEngine.Debug.Assert(goalManager != null);
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getResult() call");
return(null);
// return typename ClientGoalHandle<ActionSpec>::ResultConstPtr() ;
}
lock ( lockObject )
{
return(listHandle.GetElement().getResult());
// return listHandle.GetElement ().getResult<TResult> ();
}
}
