public bool IsServerConnected()
{
lock (gate)
{
if (!statusReceived || statusCallerId == null)
{
return(false);
}
if (!goalSubscriberCount.ContainsKey(statusCallerId))
{
return(false);
}
if (!cancelSubscriberCount.ContainsKey(statusCallerId))
{
return(false);
}
if (feedbackSubscriber.NumPublishers == 0)
{
return(false);
}
if (resultSubscriber.NumPublishers == 0)
{
return(false);
}
ROS.Debug()($"isServerConnected: Server {statusCallerId} is fully connected.");
return(true);
}
}
private void OnGoalDisconnectCallback(SingleSubscriberPublisher publisher)
{
int subscriberCount = 0;
bool keyExists = goalSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount);
if (!keyExists)
{
// This should never happen. Warning has been copied from official actionlib implementation
ROS.Warn()(
$"goalDisconnectCallback: Trying to remove {publisher.SubscriberName} from " +
"goalSubscribers, but it is not in the goalSubscribers list."
);
}
else
{
ROS.Debug()(
$"goalDisconnectCallback: Removing {publisher.SubscriberName} from goalSubscribers, " +
$"(remaining with same name: {subscriberCount - 1})"
);
if (subscriberCount <= 1)
{
goalSubscriberCount.Remove(publisher.SubscriberName);
}
else
{
goalSubscriberCount[publisher.SubscriberName] = subscriberCount - 1;
}
}
}
void setSimpleState(SimpleGoalState next_state)
{
ROS.Debug("actionlib", "Transitioning SimpleState from [%s] to [%s]",
goalState.toString(),
next_state.toString());
goalState = next_state;
}
public void PublishStatus()
{
var now = DateTime.UtcNow;
var statusArray = new GoalStatusArray();
statusArray.header = new Messages.std_msgs.Header();
statusArray.header.stamp = ROS.ToTimeMessage(now);
var goalStatuses = new List <GoalStatus>();
var idsToBeRemoved = new List <string>();
foreach (var pair in goalHandles)
{
goalStatuses.Add(pair.Value.GoalStatus);
if ((pair.Value.DestructionTime != null) && (pair.Value.DestructionTime + StatusListTimeout < now))
{
ROS.Debug()("actionlib", $"Removing server goal handle for goal id: {pair.Value.GoalId.id}");
idsToBeRemoved.Add(pair.Value.GoalId.id);
}
}
foreach (string id in idsToBeRemoved)
{
goalHandles.Remove(id);
}
}
private void CancelCallback(GoalID goalId)
{
if (!started)
{
return;
}
ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new cancel request");
if (goalId.id == null)
{
var timeZero = DateTime.UtcNow;
foreach (var valuePair in goalHandles)
{
var goalHandle = valuePair.Value;
if ((ROS.GetTime(goalId.stamp) == timeZero) || (ROS.GetTime(goalHandle.GoalId.stamp) < ROS.GetTime(goalId.stamp)))
{
if (goalHandle.SetCancelRequested() && (cancelCallback != null))
{
cancelCallback(goalHandle);
}
}
}
}
else
{
ServerGoalHandle <TGoal, TResult, TFeedback> goalHandle;
var foundGoalHandle = goalHandles.TryGetValue(goalId.id, out goalHandle);
if (foundGoalHandle)
{
if (goalHandle.SetCancelRequested() && (cancelCallback != null))
{
cancelCallback(goalHandle);
}
}
else
{
// We have not received the goal yet, prepare to cancel goal when it is received
var goalStatus = new GoalStatus
{
status = GoalStatus.RECALLING
};
goalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, null)
{
DestructionTime = ROS.GetTime(goalId.stamp)
};
//lock( lockGoalHandles )
//{
goalHandles[goalId.id] = goalHandle;
//}
}
}
// Make sure to set lastCancel based on the stamp associated with this cancel request
if (ROS.GetTime(goalId.stamp) > lastCancel)
{
lastCancel = ROS.GetTime(goalId.stamp);
}
}
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();
}
/**
* \brief Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded
*
* If the goal doesn't complete by the execute_timeout, then a preempt message is sent. This call
* then waits up to the preempt_timeout for the goal to then finish.
*
* \param goal The goal to be sent to the ActionServer
* \param execute_timeout Time to wait until a preempt is sent. 0 implies wait forever
* \param preempt_timeout Time to wait after a preempt is sent. 0 implies wait forever
* \return The state of the goal when this call is completed
*/
public SimpleClientGoalState sendGoalAndWait(AGoal goal, Duration execute_timeout, Duration preempt_timeout)
{
sendGoal(goal);
// See if the goal finishes in time
if (waitForResult(execute_timeout))
{
ROS.Debug("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec());
return(getState());
}
ROS.Debug("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec());
// It didn't finish in time, so we need to preempt it
cancelGoal();
// Now wait again and see if it finishes
if (waitForResult(preempt_timeout))
{
ROS.Debug("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec());
}
else
{
ROS.Debug("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec());
}
return(getState());
}
private void OnCancelConnectCallback(SingleSubscriberPublisher publisher)
{
int subscriberCount = 0;
bool subscriberExists = cancelSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount);
cancelSubscriberCount[publisher.SubscriberName] = (subscriberExists ? subscriberCount : 0) + 1;
ROS.Debug()($"[{ThisNode.Name}] cancelConnectCallback: Adding {publisher.SubscriberName} to cancelSubscribers");
}
public bool connectTo<T> (T input) where T : CommandHandle
{
T me = (T) this;
if ( me == null )
return false;
ROS.Debug ( "Connected output port '%s (%p)' to input port '%s (%p)'", me.getName (), me, input.getName (), input );
return ( input = (T) me.get () ).connected ();
}
public bool connectFrom<T> (T output) where T : CommandHandle
{
T me = (T) this;
if ( me == null )
return false;
ROS.Debug ( "Connected output port '%s (%p)' to input port '%s (%p)'", output.getName (), output, me.getName (), me );
return ( me = (T) output.get () ).connected ();
}
private void OnGoalConnectCallback(SingleSubscriberPublisher publisher)
{
int subscriberCount = 0;
bool keyExists = goalSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount);
goalSubscriberCount[publisher.SubscriberName] = (keyExists ? subscriberCount : 0) + 1;
ROS.Debug()($"goalConnectCallback: Adding {publisher.SubscriberName} to goalSubscribers");
}
private void OnCancelConnectCallback(SingleSubscriberPublisher publisher)
{
lock (gate)
{
bool subscriberExists = cancelSubscriberCount.TryGetValue(publisher.SubscriberName, out int subscriberCount);
cancelSubscriberCount[publisher.SubscriberName] = (subscriberExists ? subscriberCount : 0) + 1;
ROS.Debug()($"cancelConnectCallback: Adding {publisher.SubscriberName} to cancelSubscriberCount");
}
}
/**
* \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);
}
}
void statusCb(MessageEvent <gsa> statusArrayEvent)
{
ROS.Debug("actionlib", "Getting status over the wire.");
if (connectionMonitor != null)
{
connectionMonitor.processStatus(statusArrayEvent.getMessage(), statusArrayEvent.getPublisherName());
}
goalManager.updateStatuses(statusArrayEvent.getMessage());
}
/**
* \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 PublishFeedback(GoalStatus goalStatus, TFeedback feedback)
{
var newFeedback = new FeedbackActionMessage <TFeedback>();
newFeedback.Header = new Messages.std_msgs.Header();
newFeedback.Header.stamp = ROS.GetTime();
newFeedback.GoalStatus = goalStatus;
newFeedback.Feedback = feedback;
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing feedback for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}");
feedbackPublisher.publish(newFeedback);
}
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}");
}
}
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 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}");
}
}
public bool IsServerConnected()
{
if (!statusReceived)
{
ROS.Debug()("isServerConnected: Didn't receive status yet, so not connected yet");
return(false);
}
if (!goalSubscriberCount.ContainsKey(statusCallerId))
{
ROS.Debug()(
$"isServerConnected: Server {statusCallerId} has not yet subscribed to the cancel " +
"topic, so not connected yet"
);
ROS.Debug()(FormatSubscriberDebugString("goalSubscribers", goalSubscriberCount));
return(false);
}
if (!cancelSubscriberCount.ContainsKey(statusCallerId))
{
ROS.Debug()(
$"isServerConnected: Server {statusCallerId} has not yet subscribed to the cancel " +
"topic, so not connected yet"
);
ROS.Debug()(FormatSubscriberDebugString("goalSubscribers", cancelSubscriberCount));
return(false);
}
if (feedbackSubscriber.NumPublishers == 0)
{
ROS.Debug()(
$"isServerConnected: Client has not yet connected to feedback topic of server " +
$"{statusCallerId}"
);
return(false);
}
if (resultSubscriber.NumPublishers == 0)
{
ROS.Debug()(
$"isServerConnected: Client has not yet connected to feedback topic of server " +
$"{statusCallerId}"
);
return(false);
}
ROS.Debug()($"isServerConnected: Server {statusCallerId} is fully connected.");
return(true);
}
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);
}
public void PublishResult(GoalStatus goalStatus, TResult result)
{
var newResult = new ResultActionMessage <TResult>();
newResult.Header = new Messages.std_msgs.Header();
newResult.Header.stamp = ROS.GetTime();
newResult.GoalStatus = goalStatus;
if (result != null)
{
newResult.Result = result;
}
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}");
resultPublisher.publish(newResult);
PublishStatus();
}
public void addPWMToQueue(MotorPWM pwm)
{
lock ( command_queue_mutex_ )
{
if (!motor_status_.on)
{
ROS.Warn("quadrotor_propulsion", "Received new motor command. Enabled motors.");
engage();
}
ROS.Debug("quadrotor_propulsion", "Received motor command valid at " + pwm.header.Stamp.ToString());
command_queue_.Enqueue(pwm);
command_condition_.Set();
}
}
/* public static bool getMassAndInertia (NodeHandle nh, ref double mass, ref double[] inertia)
{
string robot_description = "";
if ( !nh.getParam ( "robot_description", ref robot_description ) )
{
// ROS.Error ( "getMassAndInertia() couldn't find URDF at " + nh.getNamespace () + "/robot_description" );
return false;
}
// add a using urdf, after finding and porting urdf and urdfParser and all that
ModelInterface model;
try
{
model = urdf.parseURDF ( robot_description );
}
catch (System.Exception ex)
{
// ROS.Error ( "getMassAndInertia() couldn't parse URDF at " + nh.getNamespace () + "/robot_description: " + ex.Message );
return false;
}
Inertial inertial = model.getRoot ().inertial;
if ( inertial = null || inertial.mass == null || inertial.ixx == null || inertial.iyy == null || inertial.izz == null )
{
// ROS.Error ( "getMassAndInertia() requires inertial information stored on the root link " + nh.getNamespace () + "/robot_description" );
return false;
}
mass = inertial.mass;
inertia [ 0 ] = inertial.ixx;
inertia [ 1 ] = inertial.iyy;
inertia [ 2 ] = inertial.izz;
return true;
}*/
public static bool poseWithinTolerance (Pose pose_current, Pose pose_target, double dist_tolerance, double yaw_tolerance)
{
if ( yaw_tolerance > 0.0 )
{
double yaw_current, yaw_target;
tf.net.emQuaternion q = new tf.net.emQuaternion ( pose_current.orientation );
double temp;
yaw_current = q.getRPY ().z;
q = new tf.net.emQuaternion ( pose_target.orientation );
yaw_target = q.getRPY ().z;
// tf2::fromMsg ( pose_current.orientation, q );
// tf2::Matrix3x3 ( q ).getRPY ( temp, temp, yaw_current );
// tf2::fromMsg ( pose_target.orientation, q );
// tf2::Matrix3x3 ( q ).getRPY ( temp, temp, yaw_target );
double yaw_error = yaw_current - yaw_target;
// detect wrap around pi and compensate
if (yaw_error > System.Math.PI)
{
yaw_error -= 2 * System.Math.PI;
}
else if (yaw_error < -System.Math.PI)
{
yaw_error += 2 * System.Math.PI;
}
if ( System.Math.Abs ( yaw_error ) > yaw_tolerance )
{
ROS.Debug ( "Waiting for yaw " + System.Math.Abs ( yaw_current - yaw_target ) );
return false;
}
}
if ( dist_tolerance > 0.0 )
{
UnityEngine.Vector3 v_current = new UnityEngine.Vector3 ( (float) pose_current.position.x, (float) pose_current.position.y, (float) pose_current.position.z );
UnityEngine.Vector3 v_target = new UnityEngine.Vector3 ( (float) pose_target.position.x, (float) pose_target.position.y, (float) pose_target.position.z );
if ( ( v_current - v_target ).sqrMagnitude > dist_tolerance * dist_tolerance )
{
ROS.Debug ( "Waiting for distance " + ( v_current - v_target ).sqrMagnitude );
return false;
}
}
return true;
}
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 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 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 bool SetCancelRequested()
{
ROS.Debug()("actionlib", $"Transisitoning to a cancel requested state on goal id: {GoalId.id}, stamp: {GoalId.stamp}");
bool result = false;
if (GoalStatus.status == GoalStatus.PENDING)
{
SetGoalStatus(GoalStatus.RECALLING, "RECALLING");
result = true;
}
if (GoalStatus.status == GoalStatus.ACTIVE)
{
SetGoalStatus(GoalStatus.PREEMPTING, "PREEMPTING");
result = true;
}
return(result);
}