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