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); }
/// <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 static void Main(string[] args) { Trace.Listeners.Add(new TextWriterTraceListener(Console.Out)); ROS.Init(args, "Talker"); var spinner = new SingleThreadSpinner(); NodeHandle node = new NodeHandle(); Publisher <std_msgs.String> Talker = node.Advertise <std_msgs.String>("/chatter", 1); int count = 0; while (ROS.OK && !Console.KeyAvailable) { Console.WriteLine("publishing message"); ROS.Info()("Publishing a chatter message: \"Blah blah blah " + count + "\""); String pow = new String("Blah blah blah " + (count++)); Talker.Publish(pow); spinner.SpinOnce(); Thread.Sleep(1000); } ROS.Shutdown(); }