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