/// <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;
}
