private void CancelCallback(GoalID goalId)
{
if (!started)
{
return;
}
ROS.Debug()("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.ToDateTime(goalId.stamp) == timeZero) || (ROS.ToDateTime(goalHandle.GoalId.stamp) < ROS.ToDateTime(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();
goalStatus.status = GoalStatus.RECALLING;
goalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, null);
goalHandle.DestructionTime = ROS.ToDateTime(goalId.stamp);
lock (lockGoalHandles)
{
goalHandles[goalId.id] = goalHandle;
}
}
}
// Make sure to set lastCancel based on the stamp associated with this cancel request
if (ROS.ToDateTime(goalId.stamp) > lastCancel)
{
lastCancel = ROS.ToDateTime(goalId.stamp);
}
}
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()("actionlib", $"Publishing feedback for goal with id: {goalStatus.goal_id.id} and stamp: " +
$"{new DateTimeOffset(ROS.ToDateTime(goalStatus.goal_id.stamp)).ToUnixTimeSeconds()}"
);
feedbackPublisher.Publish(newFeedback);
}
public ServerGoalHandle(IActionServer <TGoal, TResult, TFeedback> actionServer, GoalID goalId, GoalStatus goalStatus,
TGoal goal)
{
this.actionServer = actionServer;
GoalStatus = goalStatus;
GoalId = goalId;
GoalStatus.goal_id = goalId;
if ((goalId.stamp == null) || (ROS.ToDateTime(goalId.stamp) == new DateTime(1970, 1, 1, 0, 0, 0)))
{
// If stamp is not initialized
GoalStatus.goal_id.stamp = ROS.GetTime();
}
GoalStatus = goalStatus;
this.Goal = goal;
}
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()("actionlib", $"Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: " +
$"{new DateTimeOffset(ROS.ToDateTime(goalStatus.goal_id.stamp)).ToUnixTimeSeconds()}"
);
resultPublisher.Publish(newResult);
PublishStatus();
}
private void GoalCallback(GoalActionMessage <TGoal> goalAction)
{
if (!started)
{
return;
}
GoalID goalId = goalAction.GoalId;
ROS.Debug()("actionlib", "The action server has received a new goal request");
ServerGoalHandle <TGoal, TResult, TFeedback> observedGoalHandle = null;
if (goalHandles.ContainsKey(goalId.id))
{
observedGoalHandle = goalHandles[goalId.id];
}
if (observedGoalHandle != null)
{
// The goal could already be in a recalling state if a cancel came in before the goal
if (observedGoalHandle.GoalStatus.status == GoalStatus.RECALLING)
{
observedGoalHandle.GoalStatus.status = GoalStatus.RECALLED;
PublishResult(observedGoalHandle.GoalStatus, null); // Empty result
}
}
else
{
// Create and register new goal handle
GoalStatus goalStatus = new GoalStatus();
goalStatus.status = GoalStatus.PENDING;
var newGoalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId,
goalStatus, goalAction.Goal
);
newGoalHandle.DestructionTime = ROS.ToDateTime(goalId.stamp);
lock (lockGoalHandles)
{
goalHandles[goalId.id] = newGoalHandle;
}
goalCallback?.Invoke(newGoalHandle);
}
}
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");
}
}