/// <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);
}
/// <summary>
/// Callback function reacting to feedback message
/// </summary>
/// <param name="feedback">The feedback message indicating the progress of the trajectory</param>
private void OnFeedbackMessage(xamlamoveit.TrajectoryProgress feedback)
{
this.motionState = new SteppedMotionState(this.goalId.id, feedback.error_msg, (int)feedback.error_code, feedback.progress);
}
