Пример #1
0
 public void Shutdown()
 {
     if (subscriber != null)
     {
         subscriber.Shutdown();
         subscriber = null;
     }
     if (nodeHandle != null)
     {
         nodeHandle.Shutdown();
         nodeHandle = null;
     }
     ROS.Shutdown();
 }
Пример #2
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();
        }
Пример #3
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();
        }
Пример #4
0
        private void Start(int numberOfRuns)
        {
            NodeHandle clientNodeHandle = new NodeHandle();

            spinner = new SingleThreadSpinner(ROS.GlobalCallbackQueue);

            Console.WriteLine("Create client");
            actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
                                             Messages.actionlib.TestFeedback>("test_action", clientNodeHandle, 120);
            Console.WriteLine("Wait for client and server to negotiate connection");
            bool started = actionClient.WaitForActionServerToStartSpinning(new TimeSpan(0, 0, 3), spinner);

            /*TestParams.Add(new TestParameters("Reject Goal", GoalStatus.REJECTED, 0, false, null));
            *  TestParams.Add(new TestParameters("Cancel not yet accepted goal", GoalStatus.RECALLED, 0, true, null));
            *  TestParams.Add(new TestParameters("Cancel accepted goal", GoalStatus.PREEMPTED, 0, true, null));
            *  TestParams.Add(new TestParameters("Abort Goal", GoalStatus.ABORTED, 100, false, null));*/
            TestParams.Add(new TestParameters("Get Result 123", GoalStatus.SUCCEEDED, 100, false, 123));
            var successfulTestCount = 0;
            var failedTestCount     = 0;

            var      sw        = Stopwatch.StartNew();
            DateTime?firstFail = null;
            var      startTime = DateTime.Now;

            for (int i = 0; i < numberOfRuns; i++)
            {
                bool testError = false;
                if (started)
                {
                    // Console.WriteLine("Server connected, start tests");
                    foreach (var parameter in TestParams)
                    {
                        if (!TestCase(parameter.Name, parameter.ExpectedState, parameter.ExpectedFeedback,
                                      parameter.CancelGoal, parameter.ExpectedGoal))
                        {
                            testError = true;
                            if (firstFail == null)
                            {
                                firstFail = DateTime.Now;
                            }
                            break;
                        }
                    }
                }
                else
                {
                    Logger.LogError("Could not connect to server");
                    testError = true;
                }

                if (testError)
                {
                    Logger.LogError("Errors ocured during testing!");
                    failedTestCount++;
                }
                else
                {
                    successfulTestCount++;
                    //Logger.LogInformation("Testbed completed successfully");
                }
                //actionClient?.Shutdown();
                //actionClient = null;
            }

            Console.WriteLine("-----------");
            Console.WriteLine($"Test took {sw.Elapsed} StartTime: {startTime} Goals/s {numberOfRuns / sw.Elapsed.TotalSeconds}");
            Console.WriteLine($"Successful: {successfulTestCount} Failed: {failedTestCount} FirstFail: {firstFail}");
            Console.WriteLine("All done, press any key to exit");
            Console.WriteLine("-----------");

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

            Console.WriteLine("Shutdown client");
            actionClient?.Shutdown();
            clientNodeHandle.Shutdown();
        }