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