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); } }