private void OnGoalDisconnectCallback(SingleSubscriberPublisher publisher) { int subscriberCount = 0; bool keyExists = goalSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount); if (!keyExists) { // This should never happen. Warning has been copied from official actionlib implementation ROS.Warn()( $"goalDisconnectCallback: Trying to remove {publisher.SubscriberName} from " + "goalSubscribers, but it is not in the goalSubscribers list." ); } else { ROS.Debug()( $"goalDisconnectCallback: Removing {publisher.SubscriberName} from goalSubscribers, " + $"(remaining with same name: {subscriberCount - 1})" ); if (subscriberCount <= 1) { goalSubscriberCount.Remove(publisher.SubscriberName); } else { goalSubscriberCount[publisher.SubscriberName] = subscriberCount - 1; } } }
public ClientGoalHandle <ActionSpec> initGoal <TGoal> (TGoal goal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback) where TGoal : AGoal { AActionGoal actionGoal = new ActionSpec().NewActionGoal(); // AActionGoal actionGoal = new AActionGoal (); actionGoal.Header.Stamp = ROS.GetTime(); actionGoal.GoalID = idGenerator.generateID(); actionGoal.Goal = goal; // actionGoal.header.Stamp = ROS.GetTime (); // actionGoal.goal_id = idGenerator.generateID (); // actionGoal.goal = goal; CommStateMachine <ActionSpec> commStateMachine = new CommStateMachine <ActionSpec> (actionGoal, transitionCallback, feedbackCallback); lock ( lockObject ) { var handle = list.Add(commStateMachine); if (sendGoalDelegate != null) { sendGoalDelegate(actionGoal); } else { ROS.Warn("actionlib", "Possible coding error: sendGoalDelegate set to NULL. Not going to send goal"); } return(new ClientGoalHandle <ActionSpec> (this, handle, guard)); } }
private async void CheckDoneAsync() { await Task.Delay(this.actionClient.PreemptTimeout ?? 3000); if (this.State != CommunicationState.DONE) { ROS.Warn()($"[{ThisNode.Name}] [actionlib] Did not receive cancel acknowledgement for canceled goal id {this.Id}. Assuming that action server has been shutdown."); this.actionClient.ProcessLost(this); } }
private void Update(tfMessage msg) { foreach (TransformStamped tf in msg.transforms) { if (!setTransform(new Transform(tf))) { ROS.Warn()("Failed to setTransform in transformer update function"); } } }
public void ProcessLost(ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle) { ROS.Warn()("Transitioning goal to LOST"); if (goalHandle.LatestGoalStatus != null) { goalHandle.LatestGoalStatus.status = GoalStatus.LOST; goalHandle.LatestGoalStatus.text = "LOST"; } TransitionToState(goalHandle, CommunicationState.DONE); }
/** * \brief Register a new resource. * If the resource name already exists, the previously stored resource value will be replaced with \e val. * \param handle Resource value. Its type should implement a <tt>std::string getName()</tt> method. */ public void registerHandle(ResourceHandle handle) { if (!resource_map_.ContainsKey(handle.getName())) { resource_map_.Add(handle.getName(), handle); } else { ROS.Warn("Replacing previously registered handle '" + handle.getName() + "' in '" + this.GetType() + "'."); resource_map_[handle.getName()] = handle; } }
public void addPWMToQueue(MotorPWM pwm) { lock ( command_queue_mutex_ ) { if (!motor_status_.on) { ROS.Warn("quadrotor_propulsion", "Received new motor command. Enabled motors."); engage(); } ROS.Debug("quadrotor_propulsion", "Received motor command valid at " + pwm.header.Stamp.ToString()); command_queue_.Enqueue(pwm); command_condition_.Set(); } }
/** * \brief Blocks until this goal finishes * \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout. * \return True if the goal finished. False if the goal didn't finish within the allocated timeout */ public bool waitForResult(Duration timeout) { if (goalHandle.isExpired()) { ROS.Error("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient"); return(false); } if (timeout < new Duration(new TimeData(0, 0))) { ROS.Warn("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec()); } Time timeout_time = ROS.GetTime() + timeout; lock ( doneMutex ) { // Hardcode how often we check for node.ok() Duration loop_period = new Duration().fromSec(.1); while (nodeHandle.ok) { // Determine how long we should wait Duration time_left = timeout_time - ROS.GetTime(); // Check if we're past the timeout time if (timeout > new Duration() && time_left <= new Duration()) { break; } if (goalState == SimpleGoalState.StateEnum.DONE) { break; } // Truncate the time left if (time_left > loop_period || timeout == new Duration()) { time_left = loop_period; } } // doneCondition.timed_wait(lock, boost.posix_time.milliseconds(time_left.toSec() * 1000.0f)); } return(goalState == SimpleGoalState.StateEnum.DONE); }
private bool CheckActionServerStatusAndLog() { lock (gate) { if (IsServerConnected()) { return(true); } ROS.Warn()($"ActionServer {this.Name} is not ready (status received: {statusReceived}; callerId: {statusCallerId}; " + $"goal: {statusCallerId != null && goalSubscriberCount.ContainsKey(statusCallerId)} ({goalSubscriberCount.Count}); " + $"cancel: {statusCallerId != null && cancelSubscriberCount.ContainsKey(statusCallerId)} ({cancelSubscriberCount.Count}); " + $"feedback: {feedbackSubscriber.NumPublishers}; result: {resultSubscriber.NumPublishers})."); } return(false); }
public void cancelConnectCallback(SingleSubscriberPublisher pub) { lock ( lockObject ) { if (cancelSubscribers.ContainsKey(pub.subscriber_name)) { ROS.Warn("cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list", pub.subscriber_name); cancelSubscribers [pub.subscriber_name]++; } else { ROS.Debug("cancelConnectCallback: Adding [%s] to cancelSubscribers", pub.subscriber_name); cancelSubscribers [pub.subscriber_name] = 1; } ROS.Debug("%s", cancelSubscribersString()); } // check_connection_condition_.notify_all(); }
public void cancelDisconnectCallback(SingleSubscriberPublisher pub) { lock ( lockObject ) { if (cancelSubscribers.ContainsKey(pub.subscriber_name)) { ROS.Debug("cancelDisconnectCallback: Removing [%s] from cancelSubscribers", pub.subscriber_name); cancelSubscribers [pub.subscriber_name]--; if (cancelSubscribers[pub.subscriber_name] == 0) { cancelSubscribers.Remove(pub.subscriber_name); } } else { ROS.Warn("cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list", pub.subscriber_name); } } ROS.Debug("%s", cancelSubscribersString()); }
public void goalConnectCallback(SingleSubscriberPublisher pub) { lock ( lockObject ) { // check if the dictionary contains this publisher if (goalSubscribers.ContainsKey(pub.subscriber_name)) { ROS.Warn("goalConnectCallback: Trying to add [%s] to goalSubscribers, but it is already in the goalSubscribers list", pub.subscriber_name); goalSubscribers [pub.subscriber_name]++; } else { ROS.Debug("goalConnectCallback: Adding [%s] to goalSubscribers", pub.subscriber_name); goalSubscribers.Add(pub.subscriber_name, 1); } } ROS.Debug("%s", goalSubscribersString()); // notify all threads waiting on this condition. Monitor.Pulse? might not need all this // check_connection_condition_.notify_all(); }
public void goalDisconnectCallback(SingleSubscriberPublisher pub) { lock ( lockObject ) { if (goalSubscribers.ContainsKey(pub.subscriber_name)) { ROS.Debug("goalDisconnectCallback: Removing [%s] from goalSubscribers", pub.subscriber_name); goalSubscribers [pub.subscriber_name]--; if (goalSubscribers [pub.subscriber_name] == 0) { goalSubscribers.Remove(pub.subscriber_name); } Dictionary <string, int> .Enumerator en = goalSubscribers.GetEnumerator(); } else { ROS.Warn("goalDisconnectCallback: Trying to remove [%s] to goalSubscribers, but it is not in the goalSubscribers list", pub.subscriber_name); } ROS.Debug("%s", goalSubscribersString()); } }
// ********* GoalStatus Connections ********* public void processStatus(gsa status, string curStatusCallerID) { lock ( lockObject ) { if (statusReceived) { if (statusCallerID != curStatusCallerID) { ROS.Warn("processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?", statusCallerID, curStatusCallerID); statusCallerID = curStatusCallerID; } latestStatusTime = status.header.Stamp; } else { ROS.Debug("processStatus: Just got our first status message from the ActionServer at node [%s]", curStatusCallerID); statusReceived = true; statusCallerID = curStatusCallerID; latestStatusTime = status.header.Stamp; } } // check_connection_condition_.notify_all(); }
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"); } }
public void processLost(ClientGoalHandle <ActionSpec> gh) { ROS.Warn("actionlib", "Transitioning goal to LOST"); latest_goal_status_.status = gstat.LOST; transitionToState(gh, CommState.StateEnum.DONE); }
/** * \brief Get the terminal state information for this goal * * Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST * This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE * \return The terminal state */ public TerminalState getTerminalState() { if (!isActive) { ROS.Error("actionlib", "Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return(new TerminalState(TerminalState.StateEnum.LOST)); } DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard); if (!protector.isProtected()) { ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getTerminalState() call"); return(new TerminalState(TerminalState.StateEnum.LOST)); } UnityEngine.Debug.Assert(goalManager != null); lock ( lockObject ) { CommState comm_state = listHandle.GetElement().getCommState(); // CommState comm_state = listHandle.getElem()->getCommState(); if (comm_state != CommState.StateEnum.DONE) { ROS.Warn("actionlib", "Asking for the terminal state when we're in [%s]", comm_state.toString()); } Messages.actionlib_msgs.GoalStatus goal_status = listHandle.GetElement().getGoalStatus(); // Messages.actionlib_msgs.GoalStatus goal_status = listHandle.getElem()->getGoalStatus(); switch (goal_status.status) { case gstat.PENDING: case gstat.ACTIVE: case gstat.PREEMPTING: case gstat.RECALLING: ROS.Error("actionlib", "Asking for terminal state, but latest goal status is %u", goal_status.status); return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text)); case gstat.PREEMPTED: return(new TerminalState(TerminalState.StateEnum.PREEMPTED, goal_status.text)); case gstat.SUCCEEDED: return(new TerminalState(TerminalState.StateEnum.SUCCEEDED, goal_status.text)); case gstat.ABORTED: return(new TerminalState(TerminalState.StateEnum.ABORTED, goal_status.text)); case gstat.REJECTED: return(new TerminalState(TerminalState.StateEnum.REJECTED, goal_status.text)); case gstat.RECALLED: return(new TerminalState(TerminalState.StateEnum.RECALLED, goal_status.text)); case gstat.LOST: return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text)); default: ROS.Error("actionlib", "Unknown goal status: %u", goal_status.status); break; } ROS.Error("actionlib", "Bug in determining terminal state"); return(new TerminalState(TerminalState.StateEnum.LOST, goal_status.text)); } }