private void StartRos(int runNumber)
{
ROS.Info()($"Start ROS #{runNumber}");
ROS.Init(new string[] { }, "PubSubTestbed");
spinner = new AsyncSpinner();
spinner.Start();
ROS.Info()("Started");
}
private void StartRos(int runNumber)
{
Logger.LogInformation($"Start ROS #{runNumber}");
ROS.Init(new string[] { }, "PubSubTestbed");
spinner = new AsyncSpinner();
spinner.Start();
Logger.LogInformation("Started");
}
private void StartRos(int runNumber)
{
Logger.LogInformation($"Start ROS #{runNumber}");
ROS.ROS_MASTER_URI = "http://localhost:11311";
ROS.Init(new string[] { }, "PubSubTestbed");
spinner = new AsyncSpinner();
spinner.Start();
Logger.LogInformation("Started");
}
static void Main(string[] args)
{
string NODE_NAME = "ServiceClientTest";
try
{
ROS.Init(ref args, NODE_NAME + DateTime.Now.Ticks);
var spinner = new AsyncSpinner();
spinner.Start();
}
catch (RosException e)
{
ROS.Critical()("ROS.Init failed, shutting down: {0}", e.Message);
ROS.shutdown();
ROS.waitForShutdown();
return;
}
try
{
var nodeHandle = new NodeHandle();
while (ROS.ok)
{
Random r = new Random();
TwoInts.Request req = new TwoInts.Request()
{
a = r.Next(100), b = r.Next(100)
};
TwoInts.Response resp = new TwoInts.Response();
DateTime before = DateTime.Now;
bool res = nodeHandle.serviceClient <TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
TimeSpan dif = DateTime.Now.Subtract(before);
string str = "";
if (res)
{
str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
}
else
{
str = "call failed after ";
}
str += Math.Round(dif.TotalMilliseconds, 2) + " ms";
ROS.Info()(str);
Thread.Sleep(1000);
}
}
catch (RosException e)
{
ROS.Critical()("Shutting down: {0}", e.Message);
}
ROS.shutdown();
ROS.waitForShutdown();
}
public RosoutDebug(int verboseLevel, string[] args)
{
this.verboseLevel = verboseLevel;
ROS.Init(args, "RosoutDebug");
var asyncSpinner = new AsyncSpinner();
asyncSpinner.Start();
nodeHandle = new NodeHandle();
Init();
}
private static void Main(string[] args)
{
Trace.Listeners.Add(new TextWriterTraceListener(Console.Out));
ROS.Init(args, "Listener");
var spinner = new AsyncSpinner();
spinner.Start();
NodeHandle node = new NodeHandle();
Subscriber Subscriber = node.subscribe <std_msgs.String>("/chatter", 1, chatterCallback);
ROS.waitForShutdown();
}
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();
}
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();
}
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)
{
NodeHandle nodeHandle;
string NODE_NAME = "ServiceServerTest";
ServiceServer server;
try
{
ROS.Init(new string[0], NODE_NAME);
var spinner = new AsyncSpinner();
spinner.Start();
}
catch (RosException e)
{
ROS.Critical()("ROS.Init failed, shutting down: {0}", e.Message);
ROS.shutdown();
ROS.waitForShutdown();
return;
}
try
{
nodeHandle = new NodeHandle();
server = nodeHandle.advertiseService <TwoInts.Request, TwoInts.Response>("/add_two_ints", addition);
while (ROS.ok && server.IsValid)
{
Thread.Sleep(10);
}
}
catch (RosException e)
{
ROS.Critical()("Shutting down: {0}", e.Message);
}
ROS.shutdown();
ROS.waitForShutdown();
}
/*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();
}
static void Main(string[] args)
{
ROS.Init(args, "tf_example");
var asyncSpinner = new AsyncSpinner();
asyncSpinner.Start();
NodeHandle nh = new NodeHandle();
ROS.Info()("This node will create a Transformer to compare lookup results between four source/target " +
"frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"
);
while (ROS.OK && !ROS.ShuttingDown && (!Master.GetNodes().Result.Contains("/camera/driver")))
{
ROS.Error()("For this to work, you need to \"roslaunch openni2_launch openni2.launch\" on a PC with" +
"an ASUS Xtion or PrimeSense Carmine sensor plugged into it, and connect to the same master"
);
Thread.Sleep(2000);
}
if (ROS.OK && !ROS.ShuttingDown)
{
tfer = new tf.Transformer(false);
#region tf_echo results
/*
* tf_echo camera_link camera_rgb_frame
* (0.0,-0.045,0.0)
* (0,0,0,1)
*/
tf.Transform result1 = new tf.Transform()
{
basis = new tf.Quaternion(0, 0, 0, 1),
origin = new tf.Vector3(0, -0.045, 0),
child_frame_id = "camera_rgb_frame",
frame_id = "camera_link"
};
/*
* tf_echo camera_link camera_rgb_optical_frame
* (0.0,-0.045,0.0)
* (-0.5,0.5,-0.5,0.5)
*/
tf.Transform result2 = new tf.Transform()
{
basis = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
origin = new tf.Vector3(0.0, 0.0, 0.0),
child_frame_id = "camera_rgb_optical_frame",
frame_id = "camera_rgb_frame"
};
/*
* tf_echo camera_rgb_frame camera_depth_frame
* (0.0,0.25,0.0)
* (0,0,0,1)
*/
tf.Transform result3 = new tf.Transform()
{
basis = new tf.Quaternion(0, 0, 0, 1),
origin = new tf.Vector3(0.0, -0.02, 0.0),
child_frame_id = "camera_depth_frame",
frame_id = "camera_link"
};
/*
* tf_echo camera_rgb_optical_frame camera_depth_frame
* (-0.25,0.0,0.0)
* (0.5,-0.5,0.5,0.5)
*/
tf.Transform result4 = new tf.Transform()
{
basis = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
origin = new tf.Vector3(0.0, 0.0, 0.0),
child_frame_id = "camera_depth_optical_frame",
frame_id = "camera_depth_frame"
};
#endregion
tf.Transform test1 = null, test2 = null, test3 = null, test4 = null;
do
{
if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
{
test1 = testLookup(result1);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
{
test2 = testLookup(result2);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
{
test3 = testLookup(result3);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
{
test4 = testLookup(result4);
}
Thread.Sleep(100);
} while (ROS.OK && !ROS.ShuttingDown && (
test1 == null || !string.Equals(result1.ToString(), test1.ToString()) ||
test2 == null || !string.Equals(result2.ToString(), test2.ToString()) ||
test3 == null || !string.Equals(result3.ToString(), test3.ToString()) ||
test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
);
}
if (ROS.OK && !ROS.ShuttingDown)
{
Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
Console.ReadLine();
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();
}
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)
{
//#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();
}
