void OnMouseDown()
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
}
private static bool addition(TwoInts.Request req, ref TwoInts.Response resp)
{
ROS.Info()("[ServiceServerSample] addition callback");
resp.sum = req.a + req.b;
ROS.Info()(req.ToString());
ROS.Info()(resp.sum.ToString());
return(true);
}
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 StopRos(int runNumber)
{
ROS.Info()($"Stop ROS #{runNumber}");
spinner.Stop();
var shutdownTask = ROS.shutdown();
shutdownTask.Wait();
ROS.Info()("Stopped");
}
private void fire(int cam)
{
ROS.Info("Trying to set params for cam: " + cam);
cm msg = new cm {
brightness = SUBS[cam][0].Value, contrast = SUBS[cam][1].Value, exposure = SUBS[cam][2].Value, gain = SUBS[cam][3].Value, saturation = SUBS[cam][4].Value, wbt = SUBS[cam][5].Value, exposure_auto = exp, focus_auto = foc
};
pub[cam].publish(msg);
}
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();
}
static void Main(string[] args)
{
var testbed = new Program();
ROS.Info()("Start testing of init and shutdown of ROS");
var result = testbed.TestStartStopOfRos(10);
ROS.Info()($"Test finish. Successful: {result.Item1}, Failed: {result.Item2}");
}
public bool stop (CommandHandle handle)
{
if ( handle == null )
return false;
string resource = handle.getName ();
if ( enabled_.ContainsKey ( resource ) )
{
if ( enabled_ [ resource ] == handle )
{
enabled_.Remove ( resource );
ROS.Info ( "quadrotor_interface", "Disabled %s control", resource );
return true;
}
}
return false;
}
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();
}
public bool start (CommandHandle handle)
{
if ( handle == null || !handle.connected () )
return false;
string resource = handle.getName ();
if ( enabled_.ContainsKey ( resource ) )
{
if ( enabled_ [ resource ] == handle )
return true;
} else
{
enabled_.Add ( resource, handle );
ROS.Info ( "quadrotor_interface", "Enabled %s output", resource );
return true;
}
return false;
}
public async Task <TResult> SendGoalAsync(
TGoal goal,
Action <ClientGoalHandle <TGoal, TResult, TFeedback> > onTransistionCallback = null,
Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > onFeedbackCallback = null,
CancellationToken cancel = default(CancellationToken)
)
{
if (!await this.WaitForActionServerToStartAsync(this.ActionServerWaitTimeout, cancel).ConfigureAwait(false))
{
ROS.Info()($"Action server {this.Name} is not available.");
throw new TimeoutException($"Action server {this.Name} is not available.");
}
var gh = this.SendGoal(goal, onTransistionCallback, onFeedbackCallback);
using (cancel.Register(gh.Cancel))
{
return(await gh.GoalTask.ConfigureAwait(false));
}
}
private void WaitForLatchedMessage(TimeSpan timeOut)
{
var receivedMessage = false;
var s = ROS.GlobalNodeHandle.subscribe <Messages.std_msgs.String>("/PubSubTestbed", 10, (message) =>
{
ROS.Info()("--- Received message");
receivedMessage = true;
});
var startTime = DateTime.UtcNow;
while (!receivedMessage)
{
Thread.Sleep(1);
if (DateTime.UtcNow - startTime > timeOut)
{
throw new TimeoutException($"Now message retrieved withhin {timeOut}");
}
}
}
// public FieldLimiter (T value)
// {
// return limit ( value );
// }
public void init (NodeHandle nh, string field = "")
{
float fmin = float.NegativeInfinity;
float fmax = float.PositiveInfinity;
double dmin = double.NegativeInfinity;
double dmax = double.PositiveInfinity;
bool isDouble = typeof (T) == typeof (double);
string prefix = !string.IsNullOrEmpty ( field ) ? field + "/" : "";
if ( nh.hasParam ( prefix + "max" ) || nh.hasParam ( prefix + "min" ) )
{
object posinf = isDouble ? dmax : fmax;
object neginf = isDouble ? dmin : fmin;
nh.param<T> ( prefix + "max", ref max_, (T) posinf );
nh.param<T> ( prefix + "min", ref min_, (T) neginf );
// nh.param<T>(prefix + "max", ref max_, numeric_limits<T>::infinity());
// nh.param<T>(prefix + "min", ref min_, -max_);
ROS.Info ("Limits " + nh.getNamespace () + "/" + field + " initialized " + field + " with min " + min_ + " and max " + max_);
}
}
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();
}
private void OnStatusMessage(GoalStatusArray statusArray)
{
string callerId;
var timestamp = statusArray.header.stamp;
bool callerIdPresent = statusArray.connection_header.TryGetValue("callerid", out callerId);
if (callerIdPresent)
{
//ROS.Debug()( $"[{ThisNode.Name}] Getting status over the wire (callerid: {callerId}; count: {statusArray.status_list.Length})." );
if (statusReceived)
{
if (statusCallerId != callerId)
{
ROS.Warn()($"[{ThisNode.Name}] onStatusMessage: Previously received status from {statusCallerId}, but we now received status from {callerId}. Did the ActionServer change?");
statusCallerId = callerId;
}
}
else
{
ROS.Debug()($"[{ThisNode.Name}] onStatusMessage: Just got our first status message from the ActionServer at node {callerId}");
statusReceived = true;
statusCallerId = callerId;
}
LatestStatusTime = timestamp;
if (LatestSequenceNumber != null && statusArray.header.seq <= LatestSequenceNumber)
{
ROS.Warn()("[{ThisNode.Name}] Status sequence number was decreased. This can only happen when the action server was restarted. Assume all active goals are lost.");
handleConnectionLost();
}
LatestSequenceNumber = statusArray.header.seq;
// Create a copy of all goal handle references in thread safe environment so it can be looped over all goal
// handles without blocking the sending of new goals
Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> > goalHandlesReferenceCopy;
lock ( lockGoalHandles )
{
goalHandlesReferenceCopy = new Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> >(goalHandles);
}
// Loop over all goal handles and update their state, mark goal handles that are done for deletion
var completedGoals = new List <string>();
foreach (var pair in goalHandlesReferenceCopy)
{
if ((pair.Value.LatestResultAction == null) || (ROS.GetTime(pair.Value.LatestResultAction.Header.stamp) < ROS.GetTime(timestamp)))
{
var goalStatus = FindGoalInStatusList(statusArray, pair.Key);
UpdateStatus(pair.Value, goalStatus);
if (pair.Value.State == CommunicationState.DONE)
{
completedGoals.Add(pair.Key);
}
}
}
// Remove goal handles that are done from the tracking list
foreach (var goalHandleId in completedGoals)
{
ROS.Info()($"[{ThisNode.Name}] Remove goal handle id {goalHandleId} from tracked goal handles");
lock ( lockGoalHandles )
{
goalHandles.Remove(goalHandleId);
}
}
}
else
{
ROS.Error()($"[{ThisNode.Name}] Received StatusMessage with no caller ID");
}
}
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();
}
}
public void subCallback(Messages.std_msgs.String msg)
{
ROS.Info("Receieved:\n" + msg.data);
}
private static void chatterCallback(std_msgs.String s)
{
ROS.Info()("RECEIVED: " + s.data);
Console.WriteLine($"Received: " + s.data);
}
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();
}
private static void chatterCallback(m.String s)
{
ROS.Info("RECEIVED: " + s.data);
}
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();
}
