public ActionClient(string name, NodeHandle parentNodeHandle, int queueSize = 50)
{
this.Name = name;
this.QueueSize = queueSize;
this.nodeHandle = new NodeHandle(parentNodeHandle, name);
this.statusReceived = false;
this.goalHandles = new Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> >();
this.goalSubscriberCount = new Dictionary <string, int>();
this.cancelSubscriberCount = new Dictionary <string, int>();
statusSubscriber = nodeHandle.Subscribe <GoalStatusArray>("status", queueSize, OnStatusMessage);
feedbackSubscriber = nodeHandle.Subscribe <FeedbackActionMessage <TFeedback> >("feedback", queueSize, OnFeedbackMessage);
resultSubscriber = nodeHandle.Subscribe <ResultActionMessage <TResult> >("result", queueSize, OnResultMessage);
GoalPublisher = nodeHandle.Advertise <GoalActionMessage <TGoal> >(
"goal",
queueSize,
OnGoalConnectCallback,
OnGoalDisconnectCallback
);
CancelPublisher = nodeHandle.Advertise <GoalID>(
"cancel",
queueSize,
OnCancelConnectCallback,
OnCancelDisconnectCallback
);
ROS.RosShuttingDown += ROS_RosShuttingDown;
}
/// <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 static void Main(string[] args)
{
Trace.Listeners.Add(new TextWriterTraceListener(Console.Out));
ROS.Init(args, "Listener");
var spinner = new AsyncSpinner();
spinner.Start();
NodeHandle node = new NodeHandle();
Subscriber Subscriber = node.Subscribe <std_msgs.String>("/chatter", 1, chatterCallback);
ROS.WaitForShutdown();
}
private void Init()
{
while (!ROS.IsStarted())
{
Thread.Sleep(100);
}
if (nodeHandle == null)
{
nodeHandle = new NodeHandle();
}
if (subscriber == null)
{
subscriber = nodeHandle.Subscribe <Messages.rosgraph_msgs.Log>("/rosout_agg", 100, Callback);
}
}
/// <summary>
/// Initialize the jogging client
/// </summary>
public void Initialize()
{
publisherJoggingCommand = nodeHandle.Advertise <trajectory_msgs.JointTrajectory>(TOPIC_JOGGING_COMMAND, 5, false);
publisherSetpointJoggingCommand = nodeHandle.Advertise <Messages.geometry_msgs.PoseStamped>(TOPIC_SETPOINT_JOGGING_COMMAND, 5, false);
publisherTwistJoggingCommand = nodeHandle.Advertise <Messages.geometry_msgs.TwistStamped>(TOPIC_TWIST_JOGGING_COMMAND, 5, false);
toggleTracking = nodeHandle.ServiceClient <std_srvs.SetBool>(SERVICE_TOGGLE_TRACKING);
getMovegroupName = nodeHandle.ServiceClient <xamlamoveit.GetSelected>(SERVICE_GET_MOVEGROUP_NAME);
setMovegroupName = nodeHandle.ServiceClient <xamlamoveit.SetString>(SERVICE_SET_MOVEGROUP_NAME);
getEndeffectorName = nodeHandle.ServiceClient <xamlamoveit.GetSelected>(SERVICE_GET_ENDEFFECTOR_NAME);
setEndeffectorName = nodeHandle.ServiceClient <xamlamoveit.SetString>(SERVICE_SET_ENDEFFECTOR_NAME);
status = nodeHandle.ServiceClient <xamlamoveit.StatusController>(SERVICE_STATUS, true);
getVelocityScaling = nodeHandle.ServiceClient <xamlamoveit.GetFloat>(SERVICE_GET_VELOCITY_SCALING, true);
setVelocityScaling = nodeHandle.ServiceClient <xamlamoveit.SetFloat>(SERVICE_SET_VELOCITY_SCALING, true);
getFlag = nodeHandle.ServiceClient <xamlamoveit.GetFlag>(SERVICE_GET_FLAG, true);
setFlag = nodeHandle.ServiceClient <xamlamoveit.SetFlag>(SERVICE_SET_FLAG, true);
joggingFeedback = nodeHandle.Subscribe <xamlamoveit.ControllerState>(TOPIC_JOGGING_FEEDBACK, 1, HandleJoggingFeedback, false);
}
private void InitNodeHandle()
{
nh = new NodeHandle();
nh.Subscribe <tfMessage>("/tf", 0, Update);
nh.Subscribe <tfMessage>("/tf_static", 0, Update);
}