public bool IsServerConnected() { lock (gate) { if (!statusReceived || statusCallerId == null) { return(false); } if (!goalSubscriberCount.ContainsKey(statusCallerId)) { return(false); } if (!cancelSubscriberCount.ContainsKey(statusCallerId)) { return(false); } if (feedbackSubscriber.NumPublishers == 0) { return(false); } if (resultSubscriber.NumPublishers == 0) { return(false); } ROS.Debug()($"isServerConnected: Server {statusCallerId} is fully connected."); return(true); } }
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; } } }
void setSimpleState(SimpleGoalState next_state) { ROS.Debug("actionlib", "Transitioning SimpleState from [%s] to [%s]", goalState.toString(), next_state.toString()); goalState = next_state; }
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 CancelCallback(GoalID goalId) { if (!started) { return; } ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new cancel request"); if (goalId.id == null) { var timeZero = DateTime.UtcNow; foreach (var valuePair in goalHandles) { var goalHandle = valuePair.Value; if ((ROS.GetTime(goalId.stamp) == timeZero) || (ROS.GetTime(goalHandle.GoalId.stamp) < ROS.GetTime(goalId.stamp))) { if (goalHandle.SetCancelRequested() && (cancelCallback != null)) { cancelCallback(goalHandle); } } } } else { ServerGoalHandle <TGoal, TResult, TFeedback> goalHandle; var foundGoalHandle = goalHandles.TryGetValue(goalId.id, out goalHandle); if (foundGoalHandle) { if (goalHandle.SetCancelRequested() && (cancelCallback != null)) { cancelCallback(goalHandle); } } else { // We have not received the goal yet, prepare to cancel goal when it is received var goalStatus = new GoalStatus { status = GoalStatus.RECALLING }; goalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, null) { DestructionTime = ROS.GetTime(goalId.stamp) }; //lock( lockGoalHandles ) //{ goalHandles[goalId.id] = goalHandle; //} } } // Make sure to set lastCancel based on the stamp associated with this cancel request if (ROS.GetTime(goalId.stamp) > lastCancel) { lastCancel = ROS.GetTime(goalId.stamp); } }
public void Cancel() { if (!Active) { ROS.Error()($"[{ThisNode.Name}] [actionlib] Trying to cancel() on an inactive goal handle."); } if ((State == CommunicationState.WAITING_FOR_RESULT || State == CommunicationState.RECALLING || State == CommunicationState.PREEMPTING || State == CommunicationState.DONE)) { ROS.Debug()($"[{ThisNode.Name}] [actionlib] Got a cancel() request while in state {State}, so ignoring it"); return; } else if (!(State == CommunicationState.WAITING_FOR_GOAL_ACK || State == CommunicationState.PENDING || State == CommunicationState.ACTIVE || State == CommunicationState.WAITING_FOR_CANCEL_ACK)) { ROS.Debug()($"[{ThisNode.Name}] [actionlib] BUG: Unhandled CommState: {State}"); return; } var cancelMessage = new GoalID(); cancelMessage.id = Id; actionClient.CancelPublisher.publish(cancelMessage); actionClient.TransitionToState(this, CommunicationState.WAITING_FOR_CANCEL_ACK); CheckDoneAsync(); }
/** * \brief Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded * * If the goal doesn't complete by the execute_timeout, then a preempt message is sent. This call * then waits up to the preempt_timeout for the goal to then finish. * * \param goal The goal to be sent to the ActionServer * \param execute_timeout Time to wait until a preempt is sent. 0 implies wait forever * \param preempt_timeout Time to wait after a preempt is sent. 0 implies wait forever * \return The state of the goal when this call is completed */ public SimpleClientGoalState sendGoalAndWait(AGoal goal, Duration execute_timeout, Duration preempt_timeout) { sendGoal(goal); // See if the goal finishes in time if (waitForResult(execute_timeout)) { ROS.Debug("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec()); return(getState()); } ROS.Debug("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec()); // It didn't finish in time, so we need to preempt it cancelGoal(); // Now wait again and see if it finishes if (waitForResult(preempt_timeout)) { ROS.Debug("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec()); } else { ROS.Debug("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec()); } return(getState()); }
private void OnCancelConnectCallback(SingleSubscriberPublisher publisher) { int subscriberCount = 0; bool subscriberExists = cancelSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount); cancelSubscriberCount[publisher.SubscriberName] = (subscriberExists ? subscriberCount : 0) + 1; ROS.Debug()($"[{ThisNode.Name}] cancelConnectCallback: Adding {publisher.SubscriberName} to cancelSubscribers"); }
public bool connectTo<T> (T input) where T : CommandHandle { T me = (T) this; if ( me == null ) return false; ROS.Debug ( "Connected output port '%s (%p)' to input port '%s (%p)'", me.getName (), me, input.getName (), input ); return ( input = (T) me.get () ).connected (); }
public bool connectFrom<T> (T output) where T : CommandHandle { T me = (T) this; if ( me == null ) return false; ROS.Debug ( "Connected output port '%s (%p)' to input port '%s (%p)'", output.getName (), output, me.getName (), me ); return ( me = (T) output.get () ).connected (); }
private void OnGoalConnectCallback(SingleSubscriberPublisher publisher) { int subscriberCount = 0; bool keyExists = goalSubscriberCount.TryGetValue(publisher.SubscriberName, out subscriberCount); goalSubscriberCount[publisher.SubscriberName] = (keyExists ? subscriberCount : 0) + 1; ROS.Debug()($"goalConnectCallback: Adding {publisher.SubscriberName} to goalSubscribers"); }
private void OnCancelConnectCallback(SingleSubscriberPublisher publisher) { lock (gate) { bool subscriberExists = cancelSubscriberCount.TryGetValue(publisher.SubscriberName, out int subscriberCount); cancelSubscriberCount[publisher.SubscriberName] = (subscriberExists ? subscriberCount : 0) + 1; ROS.Debug()($"cancelConnectCallback: Adding {publisher.SubscriberName} to cancelSubscriberCount"); } }
/** * \brief Sends a goal to the ActionServer, and also registers callbacks * \param transitionCallback Callback that gets called on every client state transition * \param feedbackCallback Callback that gets called whenever feedback for this goal is received */ public ClientGoalHandle <ActionSpec> sendGoal <TGoal> (TGoal goal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback) where TGoal : AGoal { ROS.Debug("actionlib", "about to start initGoal()"); ClientGoalHandle <ActionSpec> gh = goalManager.initGoal(goal, transitionCallback, feedbackCallback); ROS.Debug("actionlib", "Done with initGoal()"); return(gh); }
public void transitionToState(ClientGoalHandle <ActionSpec> gh, CommState next_state) { ROS.Debug("actionlib", "Trying to transition to %s", next_state.toString()); setCommState(next_state); if (transitionCallback != null) { transitionCallback(gh); } }
void statusCb(MessageEvent <gsa> statusArrayEvent) { ROS.Debug("actionlib", "Getting status over the wire."); if (connectionMonitor != null) { connectionMonitor.processStatus(statusArrayEvent.getMessage(), statusArrayEvent.getPublisherName()); } goalManager.updateStatuses(statusArrayEvent.getMessage()); }
/** * \brief Sends a cancel message for this specific goal to the ActionServer * * Also transitions the Communication State Machine to WAITING_FOR_CANCEL_ACK */ public void cancel() { if (!isActive) { ROS.Error("actionlib", "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return; } UnityEngine.Debug.Assert(goalManager != null); 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 call"); return; } lock ( lockObject ) { switch (listHandle.GetElement().getCommState().state) { case CommState.StateEnum.WAITING_FOR_GOAL_ACK: case CommState.StateEnum.PENDING: case CommState.StateEnum.ACTIVE: case CommState.StateEnum.WAITING_FOR_CANCEL_ACK: break; // Continue standard processing case CommState.StateEnum.WAITING_FOR_RESULT: case CommState.StateEnum.RECALLING: case CommState.StateEnum.PREEMPTING: case CommState.StateEnum.DONE: ROS.Debug("actionlib", "Got a cancel() request while in state [%s], so ignoring it", listHandle.GetElement().getCommState().toString()); return; default: ROS.Error("actionlib", "BUG: Unhandled CommState: %u", listHandle.GetElement().getCommState().state); return; } } AActionGoal actionGoal = listHandle.GetElement().getActionGoal(); Messages.actionlib_msgs.GoalID cancelMsg = new Messages.actionlib_msgs.GoalID(); cancelMsg.stamp = ROS.GetTime(); cancelMsg.id = listHandle.GetElement().getActionGoal().GoalID.id; // cancelMsg.id = listHandle.GetElement ().getActionGoal ()->goal_id.id; if (goalManager.cancelDelegate != null) { goalManager.cancelDelegate(cancelMsg); } listHandle.GetElement().transitionToState(this, CommState.StateEnum.WAITING_FOR_CANCEL_ACK); }
public void PublishFeedback(GoalStatus goalStatus, TFeedback feedback) { var newFeedback = new FeedbackActionMessage <TFeedback>(); newFeedback.Header = new Messages.std_msgs.Header(); newFeedback.Header.stamp = ROS.GetTime(); newFeedback.GoalStatus = goalStatus; newFeedback.Feedback = feedback; ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing feedback for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}"); feedbackPublisher.publish(newFeedback); }
public void SetAborted(TResult result, string text) { text = text ?? ""; ROS.Debug()($"[{ThisNode.Name}] [actionlib] Setting status to aborted on goal, id: {GoalId.id}, stamp: {GoalId.stamp}"); if ((GoalStatus.status == GoalStatus.PREEMPTING) || (GoalStatus.status == GoalStatus.ACTIVE)) { SetGoalResult(GoalStatus.ABORTED, text, result); } else { ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: {GoalStatus.status}"); } }
public void SetRejected(TResult result, string text) { text = text ?? ""; ROS.Debug()($"[{ThisNode.Name}] [actionlib] Setting status to rejected on goal, id: {GoalId.id}, stamp: {GoalId.stamp}"); if ((GoalStatus.status == GoalStatus.PENDING) || (GoalStatus.status == GoalStatus.RECALLING)) { SetGoalResult(GoalStatus.REJECTED, text, result); } else { ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: {GoalStatus.status}"); } }
public void SetSucceded(TResult result, string text) { text = text ?? ""; ROS.Debug()("actionlib", $"Setting status to succeeded on goal, id: {GoalId.id}, stamp: {GoalId.stamp}"); if ((GoalStatus.status == GoalStatus.PREEMPTING) || (GoalStatus.status == GoalStatus.ACTIVE)) { SetGoalResult(GoalStatus.SUCCEEDED, text, result); } else { ROS.Error()("actionlib", "To transition to a succeeded state, the goal must be in a preempting or active state, " + $"it is currently in state: {GoalStatus.status}"); } }
public bool IsServerConnected() { if (!statusReceived) { ROS.Debug()("isServerConnected: Didn't receive status yet, so not connected yet"); return(false); } if (!goalSubscriberCount.ContainsKey(statusCallerId)) { ROS.Debug()( $"isServerConnected: Server {statusCallerId} has not yet subscribed to the cancel " + "topic, so not connected yet" ); ROS.Debug()(FormatSubscriberDebugString("goalSubscribers", goalSubscriberCount)); return(false); } if (!cancelSubscriberCount.ContainsKey(statusCallerId)) { ROS.Debug()( $"isServerConnected: Server {statusCallerId} has not yet subscribed to the cancel " + "topic, so not connected yet" ); ROS.Debug()(FormatSubscriberDebugString("goalSubscribers", cancelSubscriberCount)); return(false); } if (feedbackSubscriber.NumPublishers == 0) { ROS.Debug()( $"isServerConnected: Client has not yet connected to feedback topic of server " + $"{statusCallerId}" ); return(false); } if (resultSubscriber.NumPublishers == 0) { ROS.Debug()( $"isServerConnected: Client has not yet connected to feedback topic of server " + $"{statusCallerId}" ); return(false); } ROS.Debug()($"isServerConnected: Server {statusCallerId} is fully connected."); return(true); }
public ClientGoalHandle <TGoal, TResult, TFeedback> SendGoal( TGoal goal, Action <ClientGoalHandle <TGoal, TResult, TFeedback> > OnTransistionCallback = null, Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > OnFeedbackCallback = null ) { // Create Goal Message; var goalId = new GoalID(); lock (lockId) { var now = ROS.GetTime(); // Create sortable goal id goalId.id = $"{ThisNode.Name}-{nextGoalId:x08}-{now.data.sec:x08}.{now.data.nsec:x08}"; goalId.stamp = now; nextGoalId = nextGoalId + 1; } // Prepare Goal Message var goalAction = new GoalActionMessage <TGoal> { Header = new Messages.std_msgs.Header { stamp = ROS.GetTime() }, GoalId = goalId, Goal = goal }; // Register goal message var goalHandle = new ClientGoalHandle <TGoal, TResult, TFeedback>( this, goalAction, OnTransistionCallback, OnFeedbackCallback ); lock (gate) { goalHandles[goalAction.GoalId.id] = goalHandle; } // Publish goal message GoalPublisher.Publish(goalAction); ROS.Debug()("Goal published: {0}", goalHandle.Id); return(goalHandle); }
public void PublishResult(GoalStatus goalStatus, TResult result) { var newResult = new ResultActionMessage <TResult>(); newResult.Header = new Messages.std_msgs.Header(); newResult.Header.stamp = ROS.GetTime(); newResult.GoalStatus = goalStatus; if (result != null) { newResult.Result = result; } ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}"); resultPublisher.publish(newResult); PublishStatus(); }
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(); } }
/* public static bool getMassAndInertia (NodeHandle nh, ref double mass, ref double[] inertia) { string robot_description = ""; if ( !nh.getParam ( "robot_description", ref robot_description ) ) { // ROS.Error ( "getMassAndInertia() couldn't find URDF at " + nh.getNamespace () + "/robot_description" ); return false; } // add a using urdf, after finding and porting urdf and urdfParser and all that ModelInterface model; try { model = urdf.parseURDF ( robot_description ); } catch (System.Exception ex) { // ROS.Error ( "getMassAndInertia() couldn't parse URDF at " + nh.getNamespace () + "/robot_description: " + ex.Message ); return false; } Inertial inertial = model.getRoot ().inertial; if ( inertial = null || inertial.mass == null || inertial.ixx == null || inertial.iyy == null || inertial.izz == null ) { // ROS.Error ( "getMassAndInertia() requires inertial information stored on the root link " + nh.getNamespace () + "/robot_description" ); return false; } mass = inertial.mass; inertia [ 0 ] = inertial.ixx; inertia [ 1 ] = inertial.iyy; inertia [ 2 ] = inertial.izz; return true; }*/ public static bool poseWithinTolerance (Pose pose_current, Pose pose_target, double dist_tolerance, double yaw_tolerance) { if ( yaw_tolerance > 0.0 ) { double yaw_current, yaw_target; tf.net.emQuaternion q = new tf.net.emQuaternion ( pose_current.orientation ); double temp; yaw_current = q.getRPY ().z; q = new tf.net.emQuaternion ( pose_target.orientation ); yaw_target = q.getRPY ().z; // tf2::fromMsg ( pose_current.orientation, q ); // tf2::Matrix3x3 ( q ).getRPY ( temp, temp, yaw_current ); // tf2::fromMsg ( pose_target.orientation, q ); // tf2::Matrix3x3 ( q ).getRPY ( temp, temp, yaw_target ); double yaw_error = yaw_current - yaw_target; // detect wrap around pi and compensate if (yaw_error > System.Math.PI) { yaw_error -= 2 * System.Math.PI; } else if (yaw_error < -System.Math.PI) { yaw_error += 2 * System.Math.PI; } if ( System.Math.Abs ( yaw_error ) > yaw_tolerance ) { ROS.Debug ( "Waiting for yaw " + System.Math.Abs ( yaw_current - yaw_target ) ); return false; } } if ( dist_tolerance > 0.0 ) { UnityEngine.Vector3 v_current = new UnityEngine.Vector3 ( (float) pose_current.position.x, (float) pose_current.position.y, (float) pose_current.position.z ); UnityEngine.Vector3 v_target = new UnityEngine.Vector3 ( (float) pose_target.position.x, (float) pose_target.position.y, (float) pose_target.position.z ); if ( ( v_current - v_target ).sqrMagnitude > dist_tolerance * dist_tolerance ) { ROS.Debug ( "Waiting for distance " + ( v_current - v_target ).sqrMagnitude ); return false; } } return true; }
private void OnResultMessage(ResultActionMessage <TResult> result) { string goalId = result.GoalStatus.goal_id.id; ROS.Debug()("OnResultMessage (goal_id: {0}, status: {1})", goalId, result.GoalStatus.status); ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle; bool goalExists; lock (gate) { goalExists = goalHandles.TryGetValue(result.GoalStatus.goal_id.id, out goalHandle); } if (goalExists) { ROS.Debug()("Processing result for known goal handle. (goal_id: {0})", goalId); goalHandle.LatestGoalStatus = result.GoalStatus; goalHandle.LatestResultAction = result; if (goalHandle.State == CommunicationState.DONE) { ROS.Error()("Got a result when we were already in the DONE state (goal_id: {0})", goalId); } else if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK || goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK || goalHandle.State == CommunicationState.PENDING || goalHandle.State == CommunicationState.ACTIVE || goalHandle.State == CommunicationState.WAITING_FOR_RESULT || goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK || goalHandle.State == CommunicationState.RECALLING || goalHandle.State == CommunicationState.PREEMPTING) { UpdateStatus(goalHandle, result.GoalStatus); TransitionToState(goalHandle, CommunicationState.DONE); } else { ROS.Error()($"Invalid comm for result message state: {goalHandle.State}."); } } else { ROS.Debug()("Ignoring result for unknown goal (goal_id: {0})", goalId); } }
public void SetAccepted(string text) { text = text ?? ""; ROS.Debug()($"[{ThisNode.Name}] [actionlib] Accepting goal, id: {GoalId.id}, stamp: {GoalId.stamp}"); if (GoalStatus.status == GoalStatus.PENDING) { SetGoalStatus(GoalStatus.ACTIVE, text); } else if (GoalStatus.status == GoalStatus.RECALLING) { SetGoalStatus(GoalStatus.PREEMPTING, text); } else { ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: {GoalStatus.status}"); } }
public void SetCanceled(TResult result, string text) { text = text ?? ""; ROS.Debug()($"[{ThisNode.Name}] Setting status to canceled on goal, id: {GoalId.id}, stamp: {GoalId.stamp}"); if ((GoalStatus.status == GoalStatus.PENDING) || (GoalStatus.status == GoalStatus.RECALLING)) { SetGoalResult(GoalStatus.RECALLED, text, result); } else if ((GoalStatus.status == GoalStatus.ACTIVE) || (GoalStatus.status == GoalStatus.PREEMPTING)) { SetGoalResult(GoalStatus.PREEMPTED, text, result); } else { ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: {GoalStatus.status}"); } }
public void listElemDeleter(List <CommStateMachine <ActionSpec> > .Enumerator it) { UnityEngine.Debug.Assert(guard != null); 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. Not going to try delete the CommStateMachine associated with this goal"); return; } ROS.Debug("actionlib", "About to erase CommStateMachine"); lock ( lockObject ) { list.Remove(it.Current); } ROS.Debug("actionlib", "Done erasing CommStateMachine"); }
public bool SetCancelRequested() { ROS.Debug()("actionlib", $"Transisitoning to a cancel requested state on goal id: {GoalId.id}, stamp: {GoalId.stamp}"); bool result = false; if (GoalStatus.status == GoalStatus.PENDING) { SetGoalStatus(GoalStatus.RECALLING, "RECALLING"); result = true; } if (GoalStatus.status == GoalStatus.ACTIVE) { SetGoalStatus(GoalStatus.PREEMPTING, "PREEMPTING"); result = true; } return(result); }