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