/*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);
}
