public void Shutdown() { if (subscriber != null) { subscriber.Shutdown(); subscriber = null; } if (nodeHandle != null) { nodeHandle.Shutdown(); nodeHandle = null; } ROS.Shutdown(); }
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(); }
/*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(); }
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(); }