Beispiel #1
0
        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;
        }
Beispiel #2
0
        /// <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);
        }
Beispiel #3
0
        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();
        }
Beispiel #4
0
 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);
     }
 }
Beispiel #5
0
 /// <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);
 }
Beispiel #6
0
 private void InitNodeHandle()
 {
     nh = new NodeHandle();
     nh.Subscribe <tfMessage>("/tf", 0, Update);
     nh.Subscribe <tfMessage>("/tf_static", 0, Update);
 }