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;
}
}
}
public ClientGoalHandle <ActionSpec> initGoal <TGoal> (TGoal goal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback) where TGoal : AGoal
{
AActionGoal actionGoal = new ActionSpec().NewActionGoal();
// AActionGoal actionGoal = new AActionGoal ();
actionGoal.Header.Stamp = ROS.GetTime();
actionGoal.GoalID = idGenerator.generateID();
actionGoal.Goal = goal;
// actionGoal.header.Stamp = ROS.GetTime ();
// actionGoal.goal_id = idGenerator.generateID ();
// actionGoal.goal = goal;
CommStateMachine <ActionSpec> commStateMachine = new CommStateMachine <ActionSpec> (actionGoal, transitionCallback, feedbackCallback);
lock ( lockObject )
{
var handle = list.Add(commStateMachine);
if (sendGoalDelegate != null)
{
sendGoalDelegate(actionGoal);
}
else
{
ROS.Warn("actionlib", "Possible coding error: sendGoalDelegate set to NULL. Not going to send goal");
}
return(new ClientGoalHandle <ActionSpec> (this, handle, guard));
}
}
private async void CheckDoneAsync()
{
await Task.Delay(this.actionClient.PreemptTimeout ?? 3000);
if (this.State != CommunicationState.DONE)
{
ROS.Warn()($"[{ThisNode.Name}] [actionlib] Did not receive cancel acknowledgement for canceled goal id {this.Id}. Assuming that action server has been shutdown.");
this.actionClient.ProcessLost(this);
}
}
private void Update(tfMessage msg)
{
foreach (TransformStamped tf in msg.transforms)
{
if (!setTransform(new Transform(tf)))
{
ROS.Warn()("Failed to setTransform in transformer update function");
}
}
}
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);
}
/**
* \brief Register a new resource.
* If the resource name already exists, the previously stored resource value will be replaced with \e val.
* \param handle Resource value. Its type should implement a <tt>std::string getName()</tt> method.
*/
public void registerHandle(ResourceHandle handle)
{
if (!resource_map_.ContainsKey(handle.getName()))
{
resource_map_.Add(handle.getName(), handle);
}
else
{
ROS.Warn("Replacing previously registered handle '" + handle.getName() + "' in '" + this.GetType() + "'.");
resource_map_[handle.getName()] = handle;
}
}
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();
}
}
/**
* \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);
}
private bool CheckActionServerStatusAndLog()
{
lock (gate)
{
if (IsServerConnected())
{
return(true);
}
ROS.Warn()($"ActionServer {this.Name} is not ready (status received: {statusReceived}; callerId: {statusCallerId}; "
+ $"goal: {statusCallerId != null && goalSubscriberCount.ContainsKey(statusCallerId)} ({goalSubscriberCount.Count}); "
+ $"cancel: {statusCallerId != null && cancelSubscriberCount.ContainsKey(statusCallerId)} ({cancelSubscriberCount.Count}); "
+ $"feedback: {feedbackSubscriber.NumPublishers}; result: {resultSubscriber.NumPublishers}).");
}
return(false);
}
public void cancelConnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
if (cancelSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Warn("cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name]++;
}
else
{
ROS.Debug("cancelConnectCallback: Adding [%s] to cancelSubscribers", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name] = 1;
}
ROS.Debug("%s", cancelSubscribersString());
}
// check_connection_condition_.notify_all();
}
public void cancelDisconnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
if (cancelSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Debug("cancelDisconnectCallback: Removing [%s] from cancelSubscribers", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name]--;
if (cancelSubscribers[pub.subscriber_name] == 0)
{
cancelSubscribers.Remove(pub.subscriber_name);
}
}
else
{
ROS.Warn("cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list", pub.subscriber_name);
}
}
ROS.Debug("%s", cancelSubscribersString());
}
public void goalConnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
// check if the dictionary contains this publisher
if (goalSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Warn("goalConnectCallback: Trying to add [%s] to goalSubscribers, but it is already in the goalSubscribers list", pub.subscriber_name);
goalSubscribers [pub.subscriber_name]++;
}
else
{
ROS.Debug("goalConnectCallback: Adding [%s] to goalSubscribers", pub.subscriber_name);
goalSubscribers.Add(pub.subscriber_name, 1);
}
}
ROS.Debug("%s", goalSubscribersString());
// notify all threads waiting on this condition. Monitor.Pulse? might not need all this
// check_connection_condition_.notify_all();
}
public void goalDisconnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
if (goalSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Debug("goalDisconnectCallback: Removing [%s] from goalSubscribers", pub.subscriber_name);
goalSubscribers [pub.subscriber_name]--;
if (goalSubscribers [pub.subscriber_name] == 0)
{
goalSubscribers.Remove(pub.subscriber_name);
}
Dictionary <string, int> .Enumerator en = goalSubscribers.GetEnumerator();
}
else
{
ROS.Warn("goalDisconnectCallback: Trying to remove [%s] to goalSubscribers, but it is not in the goalSubscribers list", pub.subscriber_name);
}
ROS.Debug("%s", goalSubscribersString());
}
}
// ********* GoalStatus Connections *********
public void processStatus(gsa status, string curStatusCallerID)
{
lock ( lockObject )
{
if (statusReceived)
{
if (statusCallerID != curStatusCallerID)
{
ROS.Warn("processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?",
statusCallerID, curStatusCallerID);
statusCallerID = curStatusCallerID;
}
latestStatusTime = status.header.Stamp;
}
else
{
ROS.Debug("processStatus: Just got our first status message from the ActionServer at node [%s]", curStatusCallerID);
statusReceived = true;
statusCallerID = curStatusCallerID;
latestStatusTime = status.header.Stamp;
}
}
// check_connection_condition_.notify_all();
}
private void OnStatusMessage(GoalStatusArray statusArray)
{
string callerId;
var timestamp = statusArray.header.stamp;
bool callerIdPresent = statusArray.connection_header.TryGetValue("callerid", out callerId);
if (callerIdPresent)
{
ROS.Debug()($"Getting status over the wire (callerid: {callerId}; count: " +
$"{statusArray.status_list.Length})."
);
if (statusReceived)
{
if (statusCallerId != callerId)
{
ROS.Warn()($"onStatusMessage: Previously received status from {statusCallerId}, but we now" +
$" received status from {callerId}. Did the ActionServer change?"
);
statusCallerId = callerId;
}
}
else
{
ROS.Debug()("onStatusMessage: Just got our first status message from the ActionServer at " +
$"node {callerId}"
);
statusReceived = true;
statusCallerId = callerId;
}
LatestStatusTime = timestamp;
if (LatestSequenceNumber != null && statusArray.header.seq <= LatestSequenceNumber)
{
ROS.Warn()("Status sequence number was decreased. This can only happen when the action server was restarted. Assume all active goals are lost.");
HandleConnectionLost();
}
LatestSequenceNumber = statusArray.header.seq;
// Create a copy of all goal handle references in thread safe environment so it can be looped over all goal
// handles without blocking the sending of new goals
Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> > goalHandlesReferenceCopy;
lock (lockGoalHandles)
{
goalHandlesReferenceCopy = new Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> >(goalHandles);
}
// Loop over all goal handles and update their state, mark goal handles that are done for deletion
var completedGoals = new List <string>();
foreach (var pair in goalHandlesReferenceCopy)
{
if ((pair.Value.LatestResultAction == null) || (ROS.ToDateTime(pair.Value.LatestResultAction.Header.stamp) < ROS.ToDateTime(timestamp)))
{
var goalStatus = FindGoalInStatusList(statusArray, pair.Key);
UpdateStatus(pair.Value, goalStatus);
if (pair.Value.State == CommunicationState.DONE)
{
completedGoals.Add(pair.Key);
}
}
}
// Remove goal handles that are done from the tracking list
foreach (var goalHandleId in completedGoals)
{
//Logger.LogInformation($"Remove goal handle id {goalHandleId} from tracked goal handles");
lock (lockGoalHandles)
{
goalHandles.Remove(goalHandleId);
}
}
}
else
{
ROS.Error()("Received StatusMessage with no caller ID");
}
}
public void processLost(ClientGoalHandle <ActionSpec> gh)
{
ROS.Warn("actionlib", "Transitioning goal to LOST");
latest_goal_status_.status = gstat.LOST;
transitionToState(gh, CommState.StateEnum.DONE);
}
/**
* \brief Get the terminal state information for this goal
*
* Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
* This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE
* \return The terminal state
*/
public TerminalState getTerminalState()
{
if (!isActive)
{
ROS.Error("actionlib", "Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
return(new TerminalState(TerminalState.StateEnum.LOST));
}
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 getTerminalState() call");
return(new TerminalState(TerminalState.StateEnum.LOST));
}
UnityEngine.Debug.Assert(goalManager != null);
lock ( lockObject )
{
CommState comm_state = listHandle.GetElement().getCommState();
// CommState comm_state = listHandle.getElem()->getCommState();
if (comm_state != CommState.StateEnum.DONE)
{
ROS.Warn("actionlib", "Asking for the terminal state when we're in [%s]", comm_state.toString());
}
Messages.actionlib_msgs.GoalStatus goal_status = listHandle.GetElement().getGoalStatus();
// Messages.actionlib_msgs.GoalStatus goal_status = listHandle.getElem()->getGoalStatus();
switch (goal_status.status)
{
case gstat.PENDING:
case gstat.ACTIVE:
case gstat.PREEMPTING:
case gstat.RECALLING:
ROS.Error("actionlib", "Asking for terminal state, but latest goal status is %u", goal_status.status);
return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));
case gstat.PREEMPTED:
return(new TerminalState(TerminalState.StateEnum.PREEMPTED, goal_status.text));
case gstat.SUCCEEDED:
return(new TerminalState(TerminalState.StateEnum.SUCCEEDED, goal_status.text));
case gstat.ABORTED:
return(new TerminalState(TerminalState.StateEnum.ABORTED, goal_status.text));
case gstat.REJECTED:
return(new TerminalState(TerminalState.StateEnum.REJECTED, goal_status.text));
case gstat.RECALLED:
return(new TerminalState(TerminalState.StateEnum.RECALLED, goal_status.text));
case gstat.LOST:
return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));
default:
ROS.Error("actionlib", "Unknown goal status: %u", goal_status.status);
break;
}
ROS.Error("actionlib", "Bug in determining terminal state");
return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text));
}
}
