public void Shutdown()
{
statusSubscriber.shutdown();
feedbackSubscriber.shutdown();
resultSubscriber.shutdown();
GoalPublisher.shutdown();
CancelPublisher.shutdown();
nodeHandle.shutdown();
}
public void shutdown()
{
if (dynamic != null)
{
dynamic = null;
}
if (nh != null)
{
nh.shutdown();
nh = null;
}
}
static void Main(string[] args)
{
#if (DEBUG)
Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/");
#endif
Console.WriteLine("Start ROS");
ROS.Init(ref args, "ActionServerClientSlowDummy");
ICallbackQueue callbackQueue = new CallbackQueue();
var asyncSpinner = new AsyncSpinner(callbackQueue);
asyncSpinner.Start();
NodeHandle nodeHandle = new NodeHandle(callbackQueue);
ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback> actionClient = null;
// setup action server start
Console.WriteLine("Create server");
var actionServer = new ActionServer <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback>(nodeHandle, "test_action");
Param.Set("status_list_timeout", 999.9);
actionServer.RegisterGoalCallback((sgoalHandle) =>
{
Thread thread = new Thread(() => serverGoalCallback(sgoalHandle, actionServer, actionClient));
thread.Start();
});
Console.WriteLine("Start Server");
actionServer.Start();
Console.WriteLine("Server Started");
// setup action server finish
// setup client
actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback>("test_action_slow", nodeHandle);
// send action request to serverslowdummy
Console.WriteLine("Wait for client and server to negotiate connection");
bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 3));
if (!started)
{
Console.WriteLine("Negotiation with server failed!");
}
Console.ReadLine();
actionServer.Shutdown();
nodeHandle.shutdown();
ROS.shutdown();
}
public void shutdown()
{
if (imgsub != null)
{
imgsub.shutdown();
imgsub = null;
}
if (imagehandle != null)
{
imagehandle.shutdown();
imagehandle = null;
}
}
public void shutdown()
{
if (sub != null)
{
sub.shutdown();
sub = null;
}
if (nh != null)
{
nh.shutdown();
nh = null;
}
}
public void Shutdown()
{
if (subscriber != null)
{
subscriber.shutdown();
subscriber = null;
}
if (nodeHandle != null)
{
nodeHandle.shutdown();
nodeHandle = null;
}
ROS.shutdown();
}
static void Main(string[] args)
{
#if (DEBUG)
Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://192.168.200.32:11311/");
#endif
Console.WriteLine("Start ROS");
ROS.Init(ref args, "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();
}
public static void StopROS(Action callback = null)
{
if (ROS.isStarted() && !ROS.shutting_down && !instance.stopping)
{
// instance.status = ROSStatus.Disconnected;
instance.starting = false;
instance.stopping = true;
while (nodes.Count > 0)
{
NodeHandle node = nodes.Dequeue();
node.shutdown();
node.Dispose();
}
Debug.Log("stopping ROS");
ROS.shutdown();
ROS.waitForShutdown();
}
if (callback != null)
{
callback();
}
}
/*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();
}
private void Start(int numberOfRuns)
{
NodeHandle clientNodeHandle = new NodeHandle();
spinner = new SingleThreadSpinner(ROS.GlobalCallbackQueue);
/*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;
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);
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
{
ROS.Error()("Could not connect to server");
testError = true;
}
if (testError)
{
ROS.Error()("Errors ocured during testing!");
failedTestCount++;
}
else
{
successfulTestCount++;
ROS.Info()("Testbed completed successfully");
}
}
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();
}
static void Main(string[] args)
{
//#if (DEBUG)
// Environment.SetEnvironmentVariable("ROS_HOSTNAME", "");
// Environment.SetEnvironmentVariable("ROS_IP", "192.168.200.32");
// Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://192.168.200.231:11311/");
//#endif
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, "ActionServerSlowDummy");
ICallbackQueue callbackQueue = new CallbackQueue();
var asyncSpinner = new AsyncSpinner(callbackQueue);
asyncSpinner.Start();
//var spinner = new SingleThreadSpinner(callbackQueue);
NodeHandle serverNodeHandle = new NodeHandle(callbackQueue);
Console.WriteLine("Create server");
var actionServer = new ActionServer <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback>(serverNodeHandle, "test_action_slow");
Console.WriteLine("Start Server");
Param.Set("status_list_timeout", 999.9);
actionServer.Start();
actionServer.RegisterGoalCallback((goalHandle) =>
{
Console.WriteLine($"Goal registered callback. Goal: {goalHandle.Goal.goal}");
goalHandle.SetAccepted("accepted");
new Thread(() =>
{
for (int x = 0; x < 77; x++)
{
var fb = new Messages.actionlib.TestFeedback
{
feedback = x
};
goalHandle.PublishFeedback(fb);
Thread.Sleep(100);
}
var result = new Messages.actionlib.TestResult
{
result = 123
};
goalHandle.SetGoalStatus(Messages.actionlib_msgs.GoalStatus.SUCCEEDED, "done");
actionServer.PublishResult(goalHandle.GoalStatus, result);
}).Start();
});
Console.ReadLine();
actionServer.Shutdown();
serverNodeHandle.shutdown();
ROS.shutdown();
}
