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