public void uninit()
{
ROS.shutdown();
ROS.waitForShutdown();
isConnect = false;
}
public void OnClosed()
{
closing = true;
ROS.shutdown();
autoEvent_pub_camera.Set();
autoEvent_pub_control.Set();
autoEvent_ui.Set();
vp.EasyPlayer_CloseStream();
}
private void StopRos(int runNumber)
{
ROS.Info()($"Stop ROS #{runNumber}");
spinner.Stop();
var shutdownTask = ROS.shutdown();
shutdownTask.Wait();
ROS.Info()("Stopped");
}
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();
}
protected override void OnClosed(EventArgs e)
{
if (!closing)
{
closing = true;
pubthread.Join();
}
ROS.shutdown();
ROS.waitForShutdown();
base.OnClosed(e);
}
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 (subscriber != null)
{
subscriber.shutdown();
subscriber = null;
}
if (nodeHandle != null)
{
nodeHandle.shutdown();
nodeHandle = null;
}
ROS.shutdown();
}
static void Main(string[] args)
{
Console.WriteLine("Start roscore and ActionServerTesbed.lua and press any key");
while (!Console.KeyAvailable)
{
Thread.Sleep(1);
}
Console.WriteLine("Start ROS");
ROS.Init(ref args, "ActionClient");
//(new Program()).Start(1);
(new TestActionServerKill()).Start(5);
ROS.shutdown();
}
static void Main(string[] args)
{
// Setup the logging system
var loggerFactory = new LoggerFactory();
loggerFactory.AddProvider(
new ConsoleLoggerProvider(
(string text, LogLevel logLevel) => { return(logLevel >= LogLevel.Information); }, true)
);
Logger = ApplicationLogging.CreateLogger(nameof(Main));
ROS.SetLoggerFactory(loggerFactory);
NodeHandle nodeHandle;
string NODE_NAME = "ServiceServerTest";
ServiceServer server;
try
{
ROS.Init(new string[0], NODE_NAME);
}
catch (RosException e)
{
Logger.LogCritical("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)
{
Logger.LogCritical("Shutting down: {0}", e.Message);
}
ROS.shutdown();
ROS.waitForShutdown();
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher <m.String> Talker = node.advertise <m.String>("/Chatter", 1);
//Subscriber<m.String> Subscriber = node.subscribe<m.String>("/Chatter", 1, chatterCallback);
int count = 0;
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
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();
}
}
private static void Main(string[] args)
{
Trace.Listeners.Add(new TextWriterTraceListener(Console.Out));
ROS.Init(args, "Talker");
var spinner = new SingleThreadSpinner();
NodeHandle node = new NodeHandle();
Publisher <std_msgs.String> Talker = node.advertise <std_msgs.String>("/chatter", 1);
int count = 0;
while (ROS.ok && !Console.KeyAvailable)
{
Console.WriteLine("publishing message");
ROS.Info()("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
spinner.SpinOnce();
Thread.Sleep(1000);
}
ROS.shutdown();
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher <m.String> Talker = node.advertise <m.String>("/Chatter", 1);
Subscriber <m.String> Subscriber = node.subscribe <m.String>("/Chatter", 1, chatterCallback);
int count = 0;
Console.WriteLine("PRESS ENTER TO QUIT!");
new Thread(() =>
{
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
}).Start();
Console.ReadLine();
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();
}
public static void StopROS()
{
if (ROS.isStarted() && !ROS.shutting_down && roslock.WaitOne())
{
if (ROS.isStarted() && !ROS.shutting_down)
{
Debug.Log("ROSManager is shutting down");
ROS.shutdown();
}
roslock.Set();
}
ROS.waitForShutdown();
#if LOG_TO_FILE
lock (loggerlock)
{
Application.logMessageReceived -= Application_logMessageReceived;
if (logwriter != null)
{
logwriter.Close();
logwriter = null;
}
}
#endif
}
void OnApplicationQuit()
{
ROS.shutdown();
ROS.waitForShutdown();
}
protected override void OnClosed(EventArgs e)
{
ROS.shutdown();
topicPoller.Join();
base.OnClosed(e);
}
/*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 Dispose()
{
Console.WriteLine("Shutting down ROS");
ROS.shutdown();
}
private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
{
ROS.shutdown();
ROS.waitForShutdown();
topicPoller.Join();
}
static void Main(string[] args)
{
string NODE_NAME = "ServiceClientTest";
var loggerFactory = new LoggerFactory();
loggerFactory.AddProvider(
new ConsoleLoggerProvider(
(string text, LogLevel logLevel) => { return(logLevel >= LogLevel.Debug); }, true)
);
Logger = ApplicationLogging.CreateLogger(nameof(Main));
ROS.SetLoggerFactory(loggerFactory);
try
{
ROS.Init(new string[0], NODE_NAME + DateTime.Now.Ticks);
}
catch (RosException e)
{
Logger.LogCritical("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";
Logger.LogInformation(str);
Thread.Sleep(1000);
}
}
catch (RosException e)
{
Logger.LogCritical("Shutting down: {0}", e.Message);
}
ROS.shutdown();
ROS.waitForShutdown();
}
static void Main(string[] args)
{
ROS.Init(args, "tf_example");
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");
tfer = new Transformer(false);
#region tf_echo results
/*
* tf_echo camera_link camera_rgb_frame
* (0.0,-0.045,0.0)
* (0,0,0,1)
*/
emTransform result1 = new emTransform()
{
basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(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)
*/
emTransform result2 = new emTransform()
{
basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link"
};
/*
* tf_echo camera_rgb_frame camera_depth_frame
* (0.0,0.25,0.0)
* (0,0,0,1)
*/
emTransform result3 = new emTransform()
{
basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame"
};
/*
* tf_echo camera_rgb_optical_frame camera_depth_frame
* (-0.25,0.0,0.0)
* (0.5,-0.5,0.5,0.5)
*/
emTransform result4 = new emTransform()
{
basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame"
};
#endregion
emTransform test1 = null, test2 = null, test3 = null, test4 = null;
do
{
if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
{
test1 = testLookup(result1);
}
if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
{
test2 = testLookup(result2);
}
if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
{
test3 = testLookup(result3);
}
if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
{
test4 = testLookup(result4);
}
Thread.Sleep(1000);
} while (!string.Equals(result1.ToString(), test1.ToString()) || !string.Equals(result2.ToString(), test2.ToString()) || !string.Equals(result3.ToString(), test3.ToString()) || !string.Equals(result4.ToString(), test4.ToString()));
Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
Console.ReadLine();
ROS.shutdown();
}
protected override void OnClosing(System.ComponentModel.CancelEventArgs e)
{
ROS.shutdown();
ROS.waitForShutdown();
base.OnClosing(e);
}
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"
);
string[] nodes = null;
while (ROS.ok && !ROS.shutting_down && (!Master.getNodes(ref nodes) || !nodes.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.shutting_down)
{
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.shutting_down)
{
break;
}
if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
{
test2 = testLookup(result2);
}
if (!ROS.ok || ROS.shutting_down)
{
break;
}
if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
{
test3 = testLookup(result3);
}
if (!ROS.ok || ROS.shutting_down)
{
break;
}
if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
{
test4 = testLookup(result4);
}
Thread.Sleep(100);
} while(ROS.ok && !ROS.shutting_down && (
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.shutting_down)
{
Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
Console.ReadLine();
ROS.shutdown();
}
}
protected override void OnClosed(EventArgs e)
{
ROS.shutdown();
vp.EasyPlayer_CloseStream();
base.OnClosed(e);
}
void OnApplicationQuit()
{
ROS.shutdown();
ROS.waitForShutdown();
pub_thread.Abort();
}
public void Close()
{
ROS.shutdown();
ROS.waitForShutdown();
}
protected override void OnClosed(EventArgs e)
{
ROS.shutdown();
base.OnClosed(e);
}
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();
}