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);
}
}
private void GoalCallback(GoalActionMessage <TGoal> goalAction)
{
if (!started)
{
return;
}
GoalID goalId = goalAction.GoalId;
ROS.Debug()($"[{ThisNode.Name}] [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.GetTime(goalId.stamp);
//lock( lockGoalHandles )
//{
goalHandles[goalId.id] = newGoalHandle;
//}
goalCallback?.Invoke(newGoalHandle);
}
}
