Exemple #1
0
        private void StopRos(int runNumber)
        {
            Logger.LogInformation($"Stop ROS #{runNumber}");
            spinner.Stop();
            var shutdownTask = ROS.Shutdown();

            shutdownTask.Wait();
            Logger.LogInformation("Stopped");
        }
Exemple #2
0
 public void Shutdown()
 {
     if (subscriber != null)
     {
         subscriber.Shutdown();
         subscriber = null;
     }
     if (nodeHandle != null)
     {
         nodeHandle.Shutdown();
         nodeHandle = null;
     }
     ROS.Shutdown();
 }
Exemple #3
0
        static void Main(string[] args)
        {
            Console.WriteLine("Start roscore and ActionServerTesbed.lua and press any key");
            while (!Console.KeyAvailable)
            {
                Thread.Sleep(1);
            }

            Console.WriteLine("Start ROS");
            ROS.ROS_MASTER_URI = "http://rosvita:11311";
            ROS.Init(new string[0], "ActionClient");

            (new Program()).Start(1000);
            //(new TestActionServerKill()).Start(5);
            Thread.Sleep(10000);

            ROS.Shutdown();
        }
Exemple #4
0
        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();
        }
Exemple #5
0
        static void Main(string[] args)
        {
            Console.WriteLine("Start ROS");
            ROS.Init(new string[0], "ActionServer");
            var asyncSpinner = new AsyncSpinner();

            asyncSpinner.Start();
            NodeHandle serverNodeHandle = new NodeHandle();

            Console.WriteLine("Create server");
            var actionServer = new ActionServer <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
                                                 Messages.actionlib.TestFeedback>(serverNodeHandle, "test_action");

            Console.WriteLine("Start Server");
            actionServer.Start();

            actionServer.RegisterGoalCallback((goalHandle) =>
            {
                Console.WriteLine($"Goal registered callback. Goal: {goalHandle.Goal.goal}");
                var fb      = new Messages.actionlib.TestFeedback();
                fb.feedback = 10;
                goalHandle.PublishFeedback(fb);
                Thread.Sleep(100);
                var result    = new Messages.actionlib.TestResult();
                result.result = 123;
                goalHandle.SetGoalStatus(Messages.actionlib_msgs.GoalStatus.SUCCEEDED, "done");
                actionServer.PublishResult(goalHandle.GoalStatus, result);
            });


            while (!Console.KeyAvailable)
            {
                Thread.Sleep(1);
            }

            actionServer.Shutdown();
            serverNodeHandle.Shutdown();
            ROS.Shutdown();
        }
Exemple #6
0
        static void Main(string[] args)
        {
            ROS.Init(args, "tf_example");
            var asyncSpinner = new AsyncSpinner();

            asyncSpinner.Start();
            NodeHandle nh = new NodeHandle();

            ROS.Info()("This node will create a Transformer to compare lookup results between four source/target " +
                       "frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"
                       );

            while (ROS.OK && !ROS.ShuttingDown && (!Master.GetNodes().Result.Contains("/camera/driver")))
            {
                ROS.Error()("For this to work, you need to \"roslaunch openni2_launch openni2.launch\" on a PC with" +
                            "an ASUS Xtion or PrimeSense Carmine sensor plugged into it, and connect to the same master"
                            );
                Thread.Sleep(2000);
            }
            if (ROS.OK && !ROS.ShuttingDown)
            {
                tfer = new tf.Transformer(false);

                #region tf_echo results

                /*
                 * tf_echo camera_link camera_rgb_frame
                 *      (0.0,-0.045,0.0)
                 *      (0,0,0,1)
                 */
                tf.Transform result1 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(0, 0, 0, 1),
                    origin         = new tf.Vector3(0, -0.045, 0),
                    child_frame_id = "camera_rgb_frame",
                    frame_id       = "camera_link"
                };

                /*
                 * tf_echo camera_link camera_rgb_optical_frame
                 *      (0.0,-0.045,0.0)
                 *      (-0.5,0.5,-0.5,0.5)
                 */
                tf.Transform result2 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
                    origin         = new tf.Vector3(0.0, 0.0, 0.0),
                    child_frame_id = "camera_rgb_optical_frame",
                    frame_id       = "camera_rgb_frame"
                };

                /*
                 * tf_echo camera_rgb_frame camera_depth_frame
                 *      (0.0,0.25,0.0)
                 *      (0,0,0,1)
                 */
                tf.Transform result3 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(0, 0, 0, 1),
                    origin         = new tf.Vector3(0.0, -0.02, 0.0),
                    child_frame_id = "camera_depth_frame",
                    frame_id       = "camera_link"
                };

                /*
                 * tf_echo camera_rgb_optical_frame camera_depth_frame
                 *      (-0.25,0.0,0.0)
                 *      (0.5,-0.5,0.5,0.5)
                 */
                tf.Transform result4 = new tf.Transform()
                {
                    basis          = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
                    origin         = new tf.Vector3(0.0, 0.0, 0.0),
                    child_frame_id = "camera_depth_optical_frame",
                    frame_id       = "camera_depth_frame"
                };

                #endregion

                tf.Transform test1 = null, test2 = null, test3 = null, test4 = null;
                do
                {
                    if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
                    {
                        test1 = testLookup(result1);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
                    {
                        test2 = testLookup(result2);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
                    {
                        test3 = testLookup(result3);
                    }
                    if (!ROS.OK || ROS.ShuttingDown)
                    {
                        break;
                    }
                    if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
                    {
                        test4 = testLookup(result4);
                    }
                    Thread.Sleep(100);
                } while (ROS.OK && !ROS.ShuttingDown && (
                             test1 == null || !string.Equals(result1.ToString(), test1.ToString()) ||
                             test2 == null || !string.Equals(result2.ToString(), test2.ToString()) ||
                             test3 == null || !string.Equals(result3.ToString(), test3.ToString()) ||
                             test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
                         );
            }
            if (ROS.OK && !ROS.ShuttingDown)
            {
                Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
                Console.ReadLine();
                ROS.Shutdown();
            }
        }
Exemple #7
0
 public void Dispose()
 {
     Console.WriteLine("Shutting down ROS");
     ROS.Shutdown();
 }
Exemple #8
0
        /*static void Main(string[] args)
         * {
         *  Console.WriteLine("Start ROS");
         *  ROS.Init(new string[0], "ActionClient");
         *  NodeHandle clientNodeHandle = new NodeHandle();
         *
         *  Console.WriteLine("Create client");
         *  var actionClient = new ActionClient<SingleJointPositionGoal, SingleJointPositionResult,
         *      SingleJointPositionFeedback>("ActionTest", clientNodeHandle);
         *
         *  Console.WriteLine("Wait for client and server to negotiate connection");
         *  bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 10));
         *
         *
         *  if (started || true)
         *  {
         *      var goal = new SingleJointPositionGoal();
         *      goal.position = 12.0;
         *      goal.max_velocity = 42.0;
         *
         *      Console.WriteLine("Send goal from client");
         *      var transition = false;
         *      actionClient.SendGoal(goal,
         *          (goalHandle) => { Console.WriteLine($"Transition: {goalHandle}"); transition = true; },
         *          (goalHandle, feedback) => { Console.WriteLine($"Feedback: {feedback}"); });
         *
         *
         *      Console.WriteLine("Wait for action client to receive transition");
         *      while (!transition)
         *      {
         *          Thread.Sleep(1);
         *      }
         *  } else
         *  {
         *      Console.WriteLine("Negotiation with server failed!");
         *  }
         * }*/


        static void Main(string[] args)
        {
            Console.WriteLine("Start ROS");

            ROS.Init(new string[0], "ActionClient");
            var asyncSpinner = new AsyncSpinner();

            asyncSpinner.Start();
            NodeHandle clientNodeHandle = ROS.GlobalNodeHandle;

            Console.WriteLine("Create client");
            var actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
                                                 Messages.actionlib.TestFeedback>("test_action", clientNodeHandle);

            Console.WriteLine("Wait for client and server to negotiate connection");
            bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 3));


            if (started)
            {
                int counter   = 0;
                var dict      = new Dictionary <int, Messages.actionlib.TestGoal>();
                var semaphore = new Semaphore(10, 10);

                while (!Console.KeyAvailable)
                {
                    var now = DateTime.UtcNow;
                    semaphore.WaitOne();
                    Console.WriteLine($"Waited: {DateTime.UtcNow - now}");
                    var goal = new Messages.actionlib.TestGoal();
                    goal.goal     = counter;
                    dict[counter] = goal;
                    counter      += 1;

                    Console.WriteLine($"Send goal {goal.goal} from client");
                    var cts = new CancellationTokenSource();
                    actionClient.SendGoalAsync(goal,
                                               (goalHandle) =>
                    {
                        if (goalHandle.State == CommunicationState.DONE)
                        {
                            semaphore.Release();
                            int g      = goalHandle.Goal.Goal.goal;
                            var result = goalHandle.Result;
                            if (result != null)
                            {
                                Console.WriteLine($"Got Result for goal {g}: {goalHandle.Result.result}");
                            }
                            else
                            {
                                Console.WriteLine($"Result for goal {g} is NULL!");
                            }
                            dict.Remove(g);
                        }
                    },
                                               (goalHandle, feedback) =>
                    {
                        Console.WriteLine($"Feedback: {feedback}");
                    },
                                               cts.Token
                                               ).GetAwaiter().GetResult();
                }

                Console.WriteLine("Wait for 15s for open goals");
                var timeOut = new TimeSpan(0, 0, 15);
                var start   = DateTime.UtcNow;
                while ((DateTime.UtcNow - start <= timeOut) && (dict.Count > 0))
                {
                    Thread.Sleep(1);
                }
                if (dict.Count == 0)
                {
                    Console.WriteLine("All goals have been reached!");
                }
                else
                {
                    Console.WriteLine("TIMEOUT: There are still open goals");
                }
            }
            else
            {
                Console.WriteLine("Negotiation with server failed!");
            }

            Console.WriteLine("Shutdown ROS");
            actionClient.Shutdown();
            clientNodeHandle.Shutdown();
            ROS.Shutdown();
        }