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);
}
}
private void StatusCallback(GoalStatusArray actionGoalStatusArray)
{
if (actionGoalStatusArray.status_list.Length > 0)
{
actionStatus = (ActionStatus)actionGoalStatusArray.status_list[0].status;
}
lastStatusUpdateTime = DateTime.Now;
}
private void StatusCallback(GoalStatusArray actionGoalStatusArray)
{
if (actionGoalStatusArray.status_list.Length > 0)
{
goalStatus = actionGoalStatusArray.status_list[actionGoalStatusArray.status_list.Length - 1];
}
OnStatusUpdated();
}
private void StatusCallback(GoalStatusArray actionGoalStatusArray)
{
if (actionGoalStatusArray.status_list.Length > 0)
{
goalStatus = actionGoalStatusArray.status_list[0];
}
lastStatusUpdateTime = DateTime.Now;
OnStatusUpdated();
}
private GoalStatus FindGoalInStatusList(GoalStatusArray statusArray, string goalId)
{
for (int i = 0; i < statusArray.status_list.Length; i++)
{
if (statusArray.status_list[i].goal_id.id == goalId)
{
return(statusArray.status_list[i]);
}
}
return(null);
}
public void PublishStatus()
{
var now = DateTime.UtcNow;
//if (this.statusArray == null)
//{
// this.statusArray = new GoalStatusArray();
// this.statusArray.header = new Messages.std_msgs.Header();
//}
var statusArray = new GoalStatusArray
{
header = new Messages.std_msgs.Header()
};
statusArray.header.stamp = ROS.GetTime(now);
goalStatuses.Clear();
idsToBeRemoved.Clear();
//lock( lockGoalHandles )
//{
foreach (var pair in goalHandles)
{
goalStatuses.Add(pair.Value.GoalStatus);
if ((pair.Value.DestructionTime != null) && (pair.Value.DestructionTime + StatusListTimeout < now))
{
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Removing server goal handle for goal id: {pair.Value.GoalId.id}");
idsToBeRemoved.Add(pair.Value.GoalId.id);
}
}
statusArray.status_list = goalStatuses.ToArray();
goalStatusPublisher.publish(statusArray);
foreach (string id in idsToBeRemoved)
{
goalHandles.TryRemove(id, out var dummy);
}
//}
}
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");
}
}
private GoalStatus FindGoalInStatusList(GoalStatusArray statusArray, string goalId) => statusArray.status_list.FirstOrDefault(x => x.goal_id.id == goalId);
