public bool call(IRosMessage req, ref IRosMessage resp) { CallInfo info = new CallInfo { req = req, resp = resp, success = false, finished = false, call_finished = false, caller_thread_id = ROS.getPID() }; bool immediate = false; lock (call_queue_mutex) { if (connection.dropped) { info.call_finished = true; return(false); } if (call_queue.Count == 0 && header_written && header_read) { immediate = true; } call_queue.Enqueue(info); } if (immediate) { processNextCall(); } while (!info.finished) { info.finished_condition.WaitOne(); } lock (info.finished_mutex) { info.call_finished = true; } resp = resp.Deserialize(resp.Serialized); if (!string.IsNullOrEmpty(info.exception)) { ROS.Error("Service call failed: service [{0}] responded with an error: {1}", name, info.exception); } return(info.success); }
public SimTime() { new Thread(() => { while (!ROS.isStarted() && !ROS.shutting_down) { Thread.Sleep(100); } Thread.Sleep(1000); if (!ROS.shutting_down) { nh = new NodeHandle(); simTimeSubscriber = nh.subscribe <Messages.rosgraph_msgs.Clock>("/clock", 1, SimTimeCallback); } ROS.waitForShutdown(); simTimeSubscriber.shutdown(); }).Start(); }
private void SimTimeCallback(Messages.rosgraph_msgs.Clock time) { if (!checkedSimTime) { if (Param.get("/use_sim_time", ref simTime)) { checkedSimTime = true; if (simTime) { ROS.Warn("Switching to sim time"); } } } if (simTime && SimTimeEvent != null) { SimTimeEvent(TimeSpan.FromMilliseconds(time.clock.data.sec * 1000.0 + (time.clock.data.nsec / 100000000.0))); } }
public bool call(IRosMessage req, ref IRosMessage resp) { if (resp == null) { //instantiate null response IN CASE this call succeeds resp = IRosMessage.generate((MsgTypes)Enum.Parse(typeof(MsgTypes), req.msgtype.ToString().Replace("Request", "Response"))); } CallInfo info = new CallInfo { req = req, resp = resp, success = false, finished = false }; bool immediate = false; lock (call_queue_mutex) { if (connection.dropped) { return(false); } if (call_queue.Count == 0 && header_written && header_read) { immediate = true; } call_queue.Enqueue(info); } if (immediate) { processNextCall(); } while (!info.finished) { info.finished_condition.WaitOne(); } resp.Deserialize(resp.Serialized); if (!string.IsNullOrEmpty(info.exception)) { ROS.Error("Service call failed: service [{0}] responded with an error: {1}", name, info.exception); } return(info.success); }
public static bool exists(string service_name, bool print_failure_reason) { string mapped_name = names.resolve(service_name); string host = ""; int port = 0; if (ServiceManager.Instance.lookUpService(mapped_name, ref host, ref port)) { TcpTransport transport = new TcpTransport(); IDictionary m = new Hashtable { { "probe", "1" }, { "md5sum", "*" }, { "callerid", this_node.Name }, { "service", mapped_name } }; byte[] headerbuf = null; int size = 0; Header h = new Header(); h.Write(m, ref headerbuf, ref size); if (transport.connect(host, port)) { byte[] sizebuf = BitConverter.GetBytes(size); transport.write(sizebuf, 0, sizebuf.Length); transport.write(headerbuf, 0, size); return(true); } if (print_failure_reason) { ROS.Info("waitForService: Service[{0}] could not connect to host [{1}:{2}], waiting...", mapped_name, host, port); } } else if (print_failure_reason) { ROS.Info("waitForService: Service[{0}] has not been advertised, waiting...", mapped_name); } return(false); }
public bool EnqueueMessage(IRosMessage msg) { lock (subscriber_links_mutex) { if (Dropped) { return(false); } } uint seq = incrementSequence(); if (HasHeader) { object h = msg.GetType().GetField("header").GetValue(msg); m.Header header; if (h == null) { header = new m.Header(); } else { header = (m.Header)h; } header.seq = seq; header.stamp = ROS.GetTime(); header.frame_id = new m.String(); msg.GetType().GetField("header").SetValue(msg, header); } msg.connection_header = connection_header.Values; lock (subscriber_links_mutex) foreach (SubscriberLink sub_link in subscriber_links) { sub_link.enqueueMessage(msg, true, false); } if (Latch) { last_message = msg; } return(true); }
public void addCallbacks(SubscriberCallbacks callbacks) { lock (callbacks_mutex) { this.callbacks.Add(callbacks); if (callbacks.connect != null && callbacks.Callback != null) { lock (subscriber_links_mutex) { foreach (SubscriberLink i in subscriber_links) { CallbackInterface cb = new PeerConnDisconnCallback(callbacks.connect, i); callbacks.Callback.addCallback(cb, ROS.getPID()); } } } } }
private bool onResponseOkAndLength(Connection conn, byte[] buf, int size, bool success) { if (conn != connection || size != 5) { throw new Exception("response or length NOT OK!"); } if (!success) { return(false); } byte ok = buf[0]; int len = BitConverter.ToInt32(buf, 1); if (len > 1000000000) { ROS.Error("GIGABYTE IS TOO BIIIIG"); connection.drop(Connection.DropReason.Destructing); return(false); } lock (call_queue_mutex) { if (ok != 0) { current_call.success = true; } else { current_call.success = false; } } if (len > 0) { connection.read(len, onResponse); } else { byte[] f = new byte[0]; onResponse(conn, f, 0, true); } return(true); }
private void construct(string ns, bool validate_name) { if (!ROS.initialized) { ROS.FREAKOUT(); } collection = new NodeHandleBackingCollection(); UnresolvedNamespace = ns; Namespace = validate_name ? resolveName(ns) : resolveName(ns, true, true); ok = true; lock (nh_refcount_mutex) { if (nh_refcount == 0 && !ROS.isStarted()) { node_started_by_nh = true; ROS.start(); } ++nh_refcount; } }
public virtual void onRequestLength(Connection conn, ref byte[] buffer, uint size, bool success) { if (!success) { return; } if (conn != connection || size != 4) { throw new Exception("Invalid request length read"); } uint len = BitConverter.ToUInt32(buffer, 0); if (len > 1000000000) { ROS.Error("A message over a gigabyte was predicted... stop... being... bad."); connection.drop(Connection.DropReason.Destructing); return; } connection.read(len, onRequest); }
private bool onHeaderReceived(Connection conn, Header header) { string md5sum; if (header.Values.Contains("md5sum")) { md5sum = (string)header.Values["md5sum"]; } else { ROS.Error("TCPROS header from service server did not have required element: md5sum"); return(false); } //TODO check md5sum bool empty = false; lock (call_queue_mutex) { empty = call_queue.Count == 0; if (empty) { header_read = true; } } IsValid = true; if (!empty) { processNextCall(); header_read = true; } return(true); }
internal override CallResult Call() { if (link.connection.dropped) { return(CallResult.Invalid); } ServiceCallbackHelperParams <MReq, MRes> parms = new ServiceCallbackHelperParams <MReq, MRes> { request = new MReq().Deserialize(buffer) as MReq, response = new MRes(), connection_header = link.connection.header.Values }; try { bool ok = isp.helper.call(parms); if (ok) { link.processResponse(parms.response, true); } else { IRosMessage res = new MRes(); link.processResponse(res, false); } } catch (Exception e) { string woops = "Exception thrown while processing service call: " + e; ROS.Error(woops); link.processResponse(woops, false); return(CallResult.Invalid); } return(CallResult.Success); }
private TF_STATUS getLatestCommonTime(uint target_id, uint source_id, ref ulong time, ref string error_str) { if (target_id == source_id) { time = TimeCache.toLong(ROS.GetTime(DateTime.Now).data); return(TF_STATUS.NO_ERROR); } List <TimeAndFrameID> lct = new List <TimeAndFrameID>(); uint frame = source_id; TimeAndFrameID temp; uint depth = 0; ulong common_time = ulong.MaxValue; while (frame != 0) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameID latest = cache.getLatestTimeAndParent(); if (latest.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } lct.Add(latest); frame = latest.frame_id; if (frame == target_id) { time = common_time; if (time == ulong.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } frame = target_id; depth = 0; common_time = ulong.MaxValue; uint common_parent = 0; while (true) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameID latest = cache.getLatestTimeAndParent(); if (latest.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } foreach (TimeAndFrameID tf in lct) { if (tf.frame_id == latest.frame_id) { common_parent = tf.frame_id; break; } } frame = latest.frame_id; if (frame == source_id) { time = common_time; if (time == uint.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } if (common_parent == 0) { error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id]; return(TF_STATUS.CONNECTIVITY_ERROR); } for (int i = 0; i < lct.Count; i++) { if (lct[i].time != 0) { common_time = Math.Min(common_time, lct[i].time); } if (lct[i].frame_id == common_parent) { break; } } if (common_time == uint.MaxValue) { common_time = 0; } time = common_time; return(TF_STATUS.NO_ERROR); }
public UInt64 Get() { return(ROS.getPID()); }
internal ulong handleMessage(IRosMessage msg, bool ser, bool nocopy, IDictionary connection_header, PublisherLink link) { lock (callbacks_mutex) { ulong drops = 0; cached_deserializers.Clear(); TimeData receipt_time = ROS.GetTime().data; foreach (ICallbackInfo info in callbacks) { MsgTypes ti = info.helper.type; if (nocopy || ser) { IMessageDeserializer deserializer = null; if (cached_deserializers.ContainsKey(ti)) { deserializer = cached_deserializers[ti]; } else { deserializer = MakeDeserializer(ti, info.helper, msg, connection_header); cached_deserializers.Add(ti, deserializer); } bool was_full = false; bool nonconst_need_copy = callbacks.Count > 1; //info.helper.callback().func(msg); //info.helper.callback().pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time); info.subscription_queue.pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time); //push(info.helper, deserializer, nonconst_need_copy, ref was_full,receipt_time); if (was_full) { ++drops; } else { info.callback.addCallback(info.subscription_queue, info.Get()); } } } if (link.Latched) { LatchInfo li = new LatchInfo { message = msg, link = link, connection_header = connection_header, receipt_time = receipt_time }; if (latched_messages.ContainsKey(link)) { latched_messages[link] = li; } else { latched_messages.Add(link, li); } } cached_deserializers.Clear(); return(drops); } }
public IMessageEvent(IRosMessage msg) : this(msg, msg.connection_header, ROS.GetTime().data, true, DefaultCreator) { }
public MessageEvent (T msg) { message = msg; connectionHeader = new Dictionary<string, string> (); receiptTime = ROS.GetTime (); }
public virtual void addCallback(CallbackInterface callback) { addCallback(callback, ROS.getPID()); }
internal bool addCallback <M>(SubscriptionCallbackHelper <M> helper, string md5sum, CallbackQueueInterface queue, uint queue_size, bool allow_concurrent_callbacks, string topiclol) where M : IRosMessage, new() { lock (md5sum_mutex) { if (this.md5sum == "*" && md5sum != "*") { this.md5sum = md5sum; } } if (md5sum != "*" && md5sum != this.md5sum) { return(false); } lock (callbacks_mutex) { CallbackInfo <M> info = new CallbackInfo <M> { helper = helper, callback = queue, subscription_queue = new Callback <M>(helper.callback().func, topiclol, queue_size, allow_concurrent_callbacks) }; //if (!helper.isConst()) //{ ++nonconst_callbacks; //} callbacks.Add(info); if (latched_messages.Count > 0) { MsgTypes ti = info.helper.type; lock (publisher_links_mutex) { foreach (PublisherLink link in publisher_links) { if (link.Latched) { if (latched_messages.ContainsKey(link)) { LatchInfo latch_info = latched_messages[link]; bool was_full = false; bool nonconst_need_copy = false; //callbacks.Count > 1; info.subscription_queue.pushitgood(info.helper, latched_messages[link].message, nonconst_need_copy, ref was_full, ROS.GetTime().data); if (!was_full) { info.callback.addCallback(info.subscription_queue, info.Get()); } } } } } } } return(true); }
public override void processRequest(ref byte[] buf, int num_bytes, IServiceClientLink link) { CallbackInterface cb = new ServiceCallback(this, helper, buf, num_bytes, link, has_tracked_object, tracked_object); callback.addCallback(cb, ROS.getPID()); }