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();
}
