/// <summary> /// Cancel all goals that were stamped at and before the specified time. All goals stamped at or before `time` will be canceled /// </summary> public void CancelGoalsAtAndBeforeTime(Time time) { var cancelMessage = new GoalID(); cancelMessage.stamp = time; CancelPublisher.publish(cancelMessage); }
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(); }
public ActionGoal(Header header, GoalID goalId, PartialMessageDeserializer goalMessageDeserializer) { _goalMessageDeserializer = goalMessageDeserializer; Header = header; GoalId = goalId; }
/// <summary> /// Creates a new <c>SteppedMotionClient</c> /// </summary> /// <param name="nodeHandle">Handle of a ROS node</param> /// <param name="trajectory">Trajectory which should be executed supervised</param> /// <param name="velocityScaling">Scaling factor to reduce or increase the trajectory velocities range [0.0 - 1.0]</param> /// <param name="checkCollision">If true collision check is performed</param> /// <param name="cancel">CancellationToken</param> /// <returns>An instance of <c>StepMotionClient</c>.</returns> /// <exception cref="Exception">Thrown when ActionSever is not available.</exception> public SteppedMotionClient(NodeHandle nodeHandle, IJointTrajectory trajectory, double velocityScaling, bool checkCollision, CancellationToken cancel = default(CancellationToken)) { this.nodeHandle = nodeHandle; this.velocityScaling = velocityScaling; this.stepwiseMoveJActionClient = new ActionClient <xamlamoveit.StepwiseMoveJGoal, xamlamoveit.StepwiseMoveJResult, xamlamoveit.StepwiseMoveJFeedback>(MOVEJ_ACTION_NAME, nodeHandle); if (!this.stepwiseMoveJActionClient.WaitForActionServerToStart(TimeSpan.FromSeconds(5))) { this.stepwiseMoveJActionClient.Shutdown(); throw new Exception($"ActionServer '{MOVEJ_ACTION_NAME}' is not available."); } var g = this.stepwiseMoveJActionClient.CreateGoal(); g.check_collision = checkCollision; g.veloctiy_scaling = velocityScaling; g.trajectory.joint_names = trajectory.JointSet.ToArray(); g.trajectory.points = trajectory.Select(x => x.ToJointTrajectoryPointMessage()).ToArray(); goalHandle = this.stepwiseMoveJActionClient.SendGoal(g); cancelSubscription = cancel.Register(goalHandle.Cancel); goalId = goalHandle.GoaldId; motionState = new SteppedMotionState(goalId.id, null, 0, 0); // set initial motion state stepPublisher = nodeHandle.Advertise <xamlamoveit.Step>(STEP_TOPIC, 1); nextPublisher = nodeHandle.Advertise <GoalID>(NEXT_TOPIC, 1); previousPublisher = nodeHandle.Advertise <GoalID>(PREVIOUS_TOPIC, 1); feedbackSubscriber = nodeHandle.Subscribe <xamlamoveit.TrajectoryProgress>(FEEDBACK_TOPIC, 10, OnFeedbackMessage); }
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 cancelTransition(string goal_id) { GoalID cancel = new GoalID(); cancel.id = goal_id; JObject cancelMsg = JObject.FromObject(cancel); publisher_transition_cancel.PublishAsync(cancelMsg); }
// When the goal is cancelled by the client private void CancelCallback(GoalID goalID) { if (actionStatus == ActionStatus.ACTIVE) { UpdateAndPublishStatus(ActionStatus.PREEMPTING); action.action_goal.goal_id = goalID; CancellationHandler(); UpdateAndPublishStatus(ActionStatus.PREEMPTED); } }
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 ServerGoalHandle(IActionServer <TGoal, TResult, TFeedback> actionServer, GoalID goalId, GoalStatus goalStatus, TGoal goal) { this.actionServer = actionServer; GoalStatus = goalStatus; GoalId = goalId; GoalStatus.goal_id = goalId; if ((goalId.stamp == null) || (ROS.ToDateTime(goalId.stamp) == new DateTime(1970, 1, 1, 0, 0, 0))) { // If stamp is not initialized GoalStatus.goal_id.stamp = ROS.GetTime(); } GoalStatus = goalStatus; this.Goal = goal; }
/** * \brief Generates a unique ID * \return A unique GoalID for this action */ public GoalID generateID() { GoalID id = new GoalID(); Messages.std_msgs.Time t = ROS.GetTime(); Messages.TimeData td = ROS.GetTime().data; string s = name + "-"; lock ( lockObject ) { goalCount++; s += goalCount + "-"; } s += td.sec + "." + td.nsec; id.id = s; id.stamp = t; return(id); }
private void CancelCallback(GoalID goalID) { switch (actionStatus) { case ActionStatus.PENDING: UpdateAndPublishStatus(ActionStatus.RECALLING); OnGoalRecalling(goalID); break; case ActionStatus.ACTIVE: UpdateAndPublishStatus(ActionStatus.PREEMPTING); OnGoalPreempting(); break; default: log("Goal cannot be canceled under current state: " + actionStatus.ToString() + ". Ignored"); break; } }
private void GoalCallback(GoalActionMessage <TGoal> goalAction) { if (!started) { return; } GoalID goalId = goalAction.GoalId; ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new goal request"); ServerGoalHandle <TGoal, TResult, TFeedback> observedGoalHandle = null; if (goalHandles.ContainsKey(goalId.id)) { observedGoalHandle = goalHandles[goalId.id]; } if (observedGoalHandle != null) { // The goal could already be in a recalling state if a cancel came in before the goal if (observedGoalHandle.GoalStatus.status == GoalStatus.RECALLING) { observedGoalHandle.GoalStatus.status = GoalStatus.RECALLED; PublishResult(observedGoalHandle.GoalStatus, null); // Empty result } } else { // Create and register new goal handle GoalStatus goalStatus = new GoalStatus(); goalStatus.status = GoalStatus.PENDING; var newGoalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, goalAction.Goal ); newGoalHandle.DestructionTime = ROS.GetTime(goalId.stamp); //lock( lockGoalHandles ) //{ goalHandles[goalId.id] = newGoalHandle; //} goalCallback?.Invoke(newGoalHandle); } }
public GetMapActionGoal(Header header, GoalID goal_id, GetMapGoal goal) : base(header, goal_id) { this.goal = goal; }
public GoalStatus(GoalID goal_id, byte status, string text) { this.goal_id = goal_id; this.status = status; this.text = text; }
public FibonacciActionGoal(Header header, GoalID goal_id, FibonacciGoal goal) : base(header, goal_id) { this.goal = goal; }
public ActionGoal(Header header, GoalID goal_id) { this.header = header; this.goal_id = goal_id; }
// When the goal is cancelled by the client protected abstract void OnGoalRecalling(GoalID goalID);
public FibonacciActionGoal() { header = new Header(); goal_id = new GoalID(); goal = new FibonacciGoal(); }
public ObjectRecognitionActionGoal(Header header, GoalID goal_id, ObjectRecognitionGoal goal) : base(header, goal_id) { this.goal = goal; }
public GetMapActionGoal() { this.header = new Header(); this.goal_id = new GoalID(); this.goal = new GetMapGoal(); }
public GetMapActionGoal(Header header, GoalID goal_id, GetMapGoal goal) { this.header = header; this.goal_id = goal_id; this.goal = goal; }
public RegisterSceneObjectToMoveitActionGoal(Header header, GoalID goal_id, RegisterSceneObjectToMoveitGoal goal) : base(header, goal_id) { this.goal = goal; }
protected override void ReceiveMessage(GoalID message) { this.id = message.id; this.stamp = message.stamp; parseValues(); }
protected override void OnGoalRecalling(GoalID goalID) { // Left blank for this example }
public FollowPathActionGoal(Header header, GoalID goal_id, FollowPathGoal goal) : base(header, goal_id) { this.goal = goal; }
public BodyPoseActionGoal() { header = new Header(); goal_id = new GoalID(); goal = new BodyPoseGoal(); }
public FollowObjectMoveitActionGoal(Header header, GoalID goal_id, FollowObjectMoveitGoal goal) : base(header, goal_id) { this.goal = goal; }
public PoseMoveitActionGoal(Header header, GoalID goal_id, PoseMoveitGoal goal) : base(header, goal_id) { this.goal = goal; }
public MoveGroupSequenceActionGoal(Header header, GoalID goal_id, MoveGroupSequenceGoal goal) : base(header, goal_id) { this.goal = goal; }
public RegisterCollisionObjectsActionGoal(Header header, GoalID goal_id, RegisterCollisionObjectsGoal goal) : base(header, goal_id) { this.goal = goal; }