/*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) { Environment.SetEnvironmentVariable("ROS_HOSTNAME", "localhost"); Environment.SetEnvironmentVariable("ROS_IP", "127.0.0.1"); Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/"); Console.WriteLine("Start ROS"); ROS.Init(ref args, "ActionClient"); ICallbackQueue callbackQueue = new CallbackQueue(); var asyncSpinner = new AsyncSpinner(callbackQueue); asyncSpinner.Start(); NodeHandle clientNodeHandle = new NodeHandle(callbackQueue); //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, 60)); if (started) { int counter = 0; var dict = new Dictionary <int, Messages.actionlib.TestGoal>(); while (!Console.KeyAvailable) { var now = DateTime.UtcNow; Console.WriteLine($"Waited: {DateTime.UtcNow - now}"); var goal = new Messages.actionlib.TestGoal { goal = counter }; dict[counter] = goal; counter += 1; Console.WriteLine($"------------------> Send goal {goal.goal} from client"); var cts = new CancellationTokenSource(); var goalTask = actionClient.SendGoalAsync(goal, (goalHandle) => { if (goalHandle.State == CommunicationState.DONE) { int g = goalHandle.Goal.Goal.goal; var resultt = goalHandle.Result; if (resultt != null) { Console.WriteLine($"------------------> Got Result for goal {g}: {resultt.result}"); } else { Console.WriteLine($"Result for goal {g} is NULL!"); } dict.Remove(g); } }, (goalHandle, feedback) => { Console.WriteLine($"------------------> Got Feedback: {feedback.Feedback.feedback}"); }, cts.Token ); var waiter = goalTask.GetAwaiter(); goalTask.Wait(); Console.ReadLine(); while (!waiter.IsCompleted) { Thread.Yield(); } var result = waiter.GetResult(); break; } 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(); }
public void Start(int numberOfTries) { NodeHandle clientNodeHandle = new NodeHandle(); var spinner = new AsyncSpinner(); spinner.Start(); //var spinner = new SingleThreadSpinner(ROS.GlobalCallbackQueue); /*Console.WriteLine("Start Action Server"); * ShellHelper.Bash("screen -dmS selfkill th SelfKillingActionServer.th"); * Console.WriteLine("Ok"); * Thread.Sleep(300000);*/ Console.WriteLine("Start Action Client"); var actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback>("self_killing_action", clientNodeHandle, 10); int numberReceived = 0; int numberTimeout = 0; for (int i = 0; i < numberOfTries; i++) { bool started = actionClient.WaitForActionServerToStart(TimeSpan.FromSeconds(10)); if (started) { var g = new Messages.actionlib.TestGoal(); g.goal = 12; Console.WriteLine("Sent"); var c = new CancellationTokenSource(); //c.CancelAfter(6000); var gh = actionClient.SendGoalAsync(g, (cgh) => { }, (cgh, fb) => { }, c.Token); bool received = false; var start = DateTime.UtcNow; while (true) { if (gh.IsCompleted) { if (gh.IsCanceled) { break; } else if (gh.IsFaulted) { break; } else if (gh.Result.result == 123) { received = true; break; } } Thread.Sleep(0); //spinner.SpinOnce(); } if (received) { Console.WriteLine("Received"); numberReceived += 1; } else { Console.WriteLine("Timeout"); numberTimeout += 1; } } else { Console.WriteLine("Could not connect to Action Server"); } } Console.WriteLine($"Done Received: {numberReceived} / Timeout: {numberTimeout}"); }
/*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 bool TestCase(string testName, byte successState, int expectedNumberOfFeedback, bool cancelGoal, int?expectedGoal) { testState = TestState.Unknown; receivedFeedback = 0; Console.WriteLine(""); Console.WriteLine(testName); var goal = new Messages.actionlib.TestGoal(); goal.goal = 42; var cts = new CancellationTokenSource(); var clientHandle = actionClient.SendGoalAsync(goal, (goalHandle) => { Console.WriteLine($"Transition: {goalHandle.State} {goalHandle.LatestGoalStatus.status} {goalHandle.Result?.result}"); if (goalHandle.State == CommunicationState.DONE) { if (goalHandle.LatestGoalStatus.status == successState) { if (expectedGoal == null) { testState = TestState.Succeeded; } else { if (goalHandle.Result?.result == expectedGoal) { Console.WriteLine($"Received expected Goal Result: {goalHandle.Result?.result}"); testState = TestState.Succeeded; } else { Console.WriteLine($"Received unexpected Goal Result: {goalHandle.Result?.result}"); testState = TestState.Failed; } } } else { testState = TestState.Failed; } } }, (goalHandle, feedback) => { //Console.WriteLine($"Feedback {feedback.Feedback.feedback} {feedback.GoalStatus.status}"); receivedFeedback += 1; }, cts.Token ); Console.WriteLine($"Sent goal {clientHandle.Id}"); if (cancelGoal) { Thread.Sleep(100); Console.WriteLine("Canceling goal"); cts.Cancel(); } WaitForSuccessWithTimeOut(5, expectedNumberOfFeedback, expectedGoal); var testResult = (testState == TestState.Succeeded); var feedbackResult = (receivedFeedback == expectedNumberOfFeedback); var result = testResult; Console.WriteLine(result ? "SUCCESS" : $"FAIL transistion: {testResult} feedback: {feedbackResult}: {receivedFeedback}/{expectedNumberOfFeedback}"); Console.WriteLine(""); return(result); }