public async Task <(bool, RosMessage)> Call(RosMessage request) { if (request == null) { throw new ArgumentNullException(nameof(request)); } RosMessage response = RosMessage.Generate(request.MessageType.Replace("__Request", "__Response")); if (response == null) { throw new Exception("Response message generation failed."); } var queue = callQueue; try { var call = new CallInfo { Request = request, Response = response }; await queue.OnNext(call, cancel).ConfigureAwait(false); bool success = await call.AsyncResult; if (success) { // response is only sent on success response.Deserialize(response.Serialized); } return(success, response); } catch (Exception e) { ROS.Error()($"Service call failed: service [{name}] responded with an error: {e}"); return(false, null); } finally { if (!persistent) { queue.OnCompleted(); } } }
private void RemoveDroppedConnections() { Connection[] localDroppedConnections = null; lock ( dropped_connections_mutex ) { localDroppedConnections = dropped_connections.ToArray(); dropped_connections.Clear(); } lock ( connections_mutex ) { foreach (Connection c in localDroppedConnections) { ROS.Debug()($"[{ThisNode.Name}] Removing dropped connection: {c.CallerID}"); connections.Remove(c); } } }
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo) { var logMessage = new Log { msg = message, name = ThisNode.Name, file = callerInfo.FilePath, function = callerInfo.MemberName, line = (uint)callerInfo.LineNumber, level = (byte)level, header = new Messages.std_msgs.Header { stamp = ROS.GetTime() } }; logMessage.topics = topicManager.GetAdvertisedTopics(); queue.TryOnNext(logMessage); }
private void ThreadFunc() { TimeSpan wallDuration = new TimeSpan(0, 0, 0, 0, ROS.WallDuration); while (ROS.ok) { DateTime begin = DateTime.UtcNow; CallAvailable(ROS.WallDuration); DateTime end = DateTime.UtcNow; var remainingTime = wallDuration - (end - begin); if (remainingTime > TimeSpan.Zero) { Thread.Sleep(remainingTime); } } ROS.Debug()($"[{ThisNode.Name}] CallbackQueue thread broke out!"); }
public void setKeepAlive(bool use, int idle, int interval, int count) { if (use) { if (!TrySetKeepAlive(socket, (uint)idle, (uint)interval)) { try { socket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.KeepAlive, use); } catch (Exception e) { ROS.Error()($"[{ThisNode.Name}] {e.ToString()}"); return; } } } }
public bool setNonBlocking() { if ((flags & (int)Flags.SYNCHRONOUS) == 0) { try { socket.Blocking = false; } catch (Exception e) { ROS.Error()($"[{ThisNode.Name}] {e.ToString()}"); close(); return(false); } } return(true); }
private async Task <IDictionary <string, string> > ReadHeader() { var remoteHeader = await this.connection.ReadHeader(cancel).ConfigureAwait(false); if (!remoteHeader.TryGetValue("md5sum", out string md5sum)) { string errorMessage = "TcpRos header from service server did not have required element: md5sum"; ROS.Error()(errorMessage); throw new ConnectionError(errorMessage); } if (!string.IsNullOrEmpty(ServiceMd5Sum)) { // TODO check md5sum } return(remoteHeader); }
/// <summary> /// Sets this timers delay and period, and immediately starts it /// </summary> /// <param name="d"></param> /// <param name="p"></param> public void Start(int d, int p) { if (timer == null) { throw new NullReferenceException("Timer instance has already been disposed"); } _delay = d; _period = p; try { timer.Change(_delay, _period); _running = d != Timeout.Infinite && p != Timeout.Infinite; } catch (Exception ex) { ROS.Error()($"[{ThisNode.Name}] Error starting timer: {ex}"); } }
public bool SafePoll(int timeout, ns.SelectMode sm) { bool res = false; try { if (!disposed) { res = realSocket.Poll(timeout, sm); } } catch (ns.SocketException e) { ROS.Error()($"[{ThisNode.Name}] {e.ToString()}"); res = !disposed && sm == ns.SelectMode.SelectError; } return(res); }
public void subscribe(SubscribeOptions ops) { lock ( subcriptionsMutex ) { if (addSubCallback(ops)) { return; } if (shuttingDown) { return; } } if (string.IsNullOrEmpty(ops.md5sum)) { throw subscribeFail(ops, "with an empty md5sum"); } if (string.IsNullOrEmpty(ops.datatype)) { throw subscribeFail(ops, "with an empty datatype"); } if (ops.helper == null) { throw subscribeFail(ops, "without a callback"); } string md5sum = ops.md5sum; string datatype = ops.datatype; var s = new Subscription(ops.topic, md5sum, datatype); s.addCallback(ops.helper, ops.md5sum, ops.callback_queue, ops.queue_size, ops.allow_concurrent_callbacks, ops.topic); if (!registerSubscriber(s, ops.datatype)) { string error = $"[{ThisNode.Name}] Couldn't register subscriber on topic [{ops.topic}]"; s.shutdown(); ROS.Error()(error); throw new RosException(error); } lock ( subcriptionsMutex ) { subscriptions.Add(s); } }
public bool unsubscribe(string topic, ISubscriptionCallbackHelper sbch) { Subscription sub = null; lock ( subcriptionsMutex ) { if (shuttingDown) { return(false); } foreach (Subscription s in subscriptions) { if (s.name == topic) { sub = s; break; } } } if (sub == null) { return(false); } sub.removeCallback(sbch); if (sub.NumCallbacks == 0) { lock ( subcriptionsMutex ) { subscriptions.Remove(sub); } if (!unregisterSubscriber(topic)) { ROS.Warn()($"[{ThisNode.Name}] Couldn't unregister subscriber for topic [{topic}]"); } sub.shutdown(); return(true); } return(true); }
private void publisherUpdateCallback(XmlRpcValue parm, XmlRpcValue result) { var pubs = new List <string>(); for (int idx = 0; idx < parm[2].Count; idx++) { pubs.Add(parm[2][idx].GetString()); } if (pubUpdate(parm[1].GetString(), pubs)) { XmlRpcManager.ResponseInt(1, "", 0)(result); } else { string error = $"[{ThisNode.Name}] Unknown error while handling XmlRpc call to pubUpdate"; ROS.Error()(error); XmlRpcManager.ResponseInt(0, error, 0)(result); } }
async Task HandleConnection() { try { await Handshake().ConfigureAwait(false); while (await callQueue.MoveNext(cancel).ConfigureAwait(false)) { var call = callQueue.Current; await ProcessCall(call).ConfigureAwait(false); } connection.Socket.Shutdown(SocketShutdown.Send); connection.Close(50); } catch (System.IO.EndOfStreamException ex) { ROS.Debug()("EndOfStreamException during connection handling. Message: {0}, Stacktrace : {1}", ex.ToString(), ex.StackTrace); } catch (System.IO.IOException ex) { ROS.Debug()("IOException during connection handling. Message: {0}, Stacktrace : {1}", ex.ToString(), ex.StackTrace); } catch (Exception e) { if (!(e is OperationCanceledException)) { logger.LogDebug($"Service client for [{name}] dropped: {e.Message}"); } FlushCallQueue(e); callQueue = new AsyncQueue <CallInfo>(MAX_CALL_QUEUE_LENGTH, true); throw; } finally { logger.LogDebug($"Removing service client for [{name}] from ServiceManager."); ServiceManager.Instance.RemoveServiceServerLinkAsync(this); } }
/// <summary> /// Starts the timer with this wrapper's set delay and period. /// </summary> public void Start() { if (timer == null) { throw new NullReferenceException("Timer instance has already been disposed"); } if (running) { return; } try { timer.Change(_delay, _period); _running = true; } catch (Exception ex) { ROS.Error()($"[{ThisNode.Name}] Error starting timer: {ex}"); } }
/// <summary> /// Stops the timer from firing, while remembering its last set state and period /// </summary> public void Stop() { if (timer == null) { throw new NullReferenceException("Timer instance has already been disposed"); } if (!running) { return; } try { timer.Change(Timeout.Infinite, Timeout.Infinite); _running = false; } catch (Exception ex) { ROS.Error()($"[{ThisNode.Name}] Error starting timer: {ex}"); } }
public void addLocalConnection(Publication pub) { lock ( publisher_links_mutex ) { if (_dropped) { return; } ROS.Info()($"[{ThisNode.Name}] Creating intraprocess link for topic [{name}]"); var pub_link = new LocalPublisherLink(this, XmlRpcManager.Instance.Uri); var sub_link = new LocalSubscriberLink(pub); pub_link.setPublisher(sub_link); sub_link.SetSubscriber(pub_link); addPublisherLink(pub_link); pub.addSubscriberLink(sub_link); } }
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo) { var logMessage = new Log { msg = message, name = ThisNode.Name, file = callerInfo.FilePath, function = callerInfo.MemberName, line = (uint)callerInfo.LineNumber, level = ((byte)((int)level)), header = new Messages.std_msgs.Header() { stamp = ROS.GetTime() } }; TopicManager.Instance.getAdvertisedTopics(out logMessage.topics); lock (log_queue) log_queue.Enqueue(logMessage); }
public static bool exists(string serviceName, bool logFailureReason = false) { string mappedName = Names.Resolve(serviceName); string host = ""; int port = 0; if (ServiceManager.Instance.LookUpService(mappedName, ref host, ref port)) { var transport = new TcpTransport(); if (transport.connect(host, port)) { var m = new Dictionary <string, string> { { "probe", "1" }, { "md5sum", "*" }, { "callerid", ThisNode.Name }, { "service", mappedName } }; var h = new Header(); h.Write(m, out byte[] headerbuf, out int size); byte[] sizebuf = BitConverter.GetBytes(size); transport.write(sizebuf, 0, sizebuf.Length); transport.write(headerbuf, 0, size); return(true); } if (logFailureReason) { ROS.Info()($"[{ThisNode.Name}] waitForService: Service[{mappedName}] could not connect to host [{host}:{port}], waiting..."); } } else if (logFailureReason) { ROS.Info()($"[{ThisNode.Name}] waitForService: Service[{mappedName}] has not been advertised, waiting..."); } return(false); }
/// <summary> /// Creates a publisher with the given advertise options /// </summary> /// <typeparam name="M">Type of topic</typeparam> /// <param name="ops">Advertise options</param> /// <returns>A publisher with the specified options</returns> public Publisher <M> advertise <M>(AdvertiseOptions <M> ops) where M : RosMessage, new() { ops.topic = resolveName(ops.topic); if (ops.callbackQueue == null) { ops.callbackQueue = Callback; } var callbacks = new SubscriberCallbacks(ops.connectCB, ops.disconnectCB, ops.callbackQueue); if (TopicManager.Instance.advertise(ops, callbacks)) { var pub = new Publisher <M>(ops.topic, ops.md5Sum, ops.dataType, this, callbacks); lock ( gate ) { collection.Publishers.Add(pub); } return(pub); } ROS.Error()($"[{ThisNode.Name}] Advertisement of publisher has failed"); return(null); }
private bool onHeaderRead(Connection conn, byte[] data, int size, bool success) { Debug.Assert(conn == this); if (!success) { return(false); } string error_msg = ""; if (!header.Parse(data, (int)size, ref error_msg)) { drop(DropReason.HeaderError); return(false); } else { string error_val = ""; if (header.Values.ContainsKey("error")) { error_val = (string)header.Values["error"]; ROS.Info()($"[{ThisNode.Name}] Received error message in header for connection to [TCPROS connection to [{transport.cachedRemoteHost}]]: [{error_val}]"); drop(DropReason.HeaderError); return(false); } else { if (header_func == null) { throw new InvalidOperationException("`header_func` callback was not registered"); } transport.parseHeader(header); header_func(conn, header); } } return(true); }
public bool requestTopic(string topic, XmlRpcValue protos, ref XmlRpcValue ret) { for (int proto_idx = 0; proto_idx < protos.Count; proto_idx++) { XmlRpcValue proto = protos[proto_idx]; if (proto.Type != XmlRpcType.Array) { ROS.Error()($"[{ThisNode.Name}] requestTopic protocol list was not a list of lists"); return(false); } if (proto[0].Type != XmlRpcType.String) { ROS.Error()($"[{ThisNode.Name}] requestTopic received a protocol list in which a sublist did not start with a string"); return(false); } string proto_name = proto[0].GetString(); if (proto_name == "TCPROS") { var tcpRosParams = new XmlRpcValue("TCPROS", Network.host, ConnectionManager.Instance.TCPPort); ret.Set(0, 1); ret.Set(1, ""); ret.Set(2, tcpRosParams); return(true); } if (proto_name == "UDPROS") { ROS.Warn()($"[{ThisNode.Name}] Ignoring topics with UdpRos as protocol"); } else { ROS.Warn()($"[{ThisNode.Name}] An unsupported protocol was offered: [{proto_name}]"); } } ROS.Error()($"[{ThisNode.Name}] No supported protocol was provided"); return(false); }
public SimTime() { new Thread(() => { try { while (!ROS.isStarted() && !ROS.shutting_down) { Thread.Sleep(100); } if (!ROS.shutting_down) { nodeHandle = new NodeHandle(); simTimeSubscriber = nodeHandle.subscribe <Clock>("/clock", 1, SimTimeCallback); } } catch (Exception e) { ROS.Error()($"[{ThisNode.Name}] Caught exception in sim time thread: {e.Message}"); } }).Start(); }
private void Construct(string ns, bool validate_name) { if (!ROS.initialized) { throw new Exception("You must call ROS.Init() before instantiating the first nodehandle"); } collection = new NodeHandleBackingCollection(); UnresolvedNamespace = ns; Namespace = validate_name ? ResolveName(ns) : ResolveName(ns, true, true); OK = true; lock (gate) { if (referenceCount == 0 && !ROS.IsStarted()) { initializedRos = true; ROS.Start(); } ++referenceCount; } }
private void writeTransport() { lock ( writing ) { if (dropped) { return; } bool can_write_more = true; while (write_callback != null && can_write_more && !dropped) { int to_write = write_size - write_sent; int bytes_sent = transport.write(write_buffer, write_sent, to_write); if (bytes_sent <= 0) { return; } write_sent += (int)bytes_sent; if (bytes_sent < write_size - write_sent) { can_write_more = false; } if (write_sent == write_size && !dropped) { WriteFinishedFunc callback = write_callback; write_callback = null; write_buffer = null; write_sent = 0; write_size = 0; if (!callback(this)) { ROS.Error()($"[{ThisNode.Name}] Failed to invoke {callback.GetMethodInfo().Name}"); } } } } }
public bool initialize(Connection connection) { ROS.Debug()($"[{ThisNode.Name}] Init transport publisher link: {parent.name }"); this.connection = connection; connection.DroppedEvent += onConnectionDropped; if (connection.transport.getRequiresHeader()) { connection.setHeaderReceivedCallback(onHeaderReceived); IDictionary <string, string> header = new Dictionary <string, string>(); header["topic"] = parent.name; header["md5sum"] = parent.md5sum; header["callerid"] = ThisNode.Name; header["type"] = parent.datatype; header["tcp_nodelay"] = "1"; connection.writeHeader(header, onHeaderWritten); } else { connection.read(4, onMessageLength); } return(true); }
public static List <string> List() { var ret = new List <string>(); var parm = new XmlRpcValue(); var result = new XmlRpcValue(); var payload = new XmlRpcValue(); parm.Set(0, ThisNode.Name); if (!Master.execute("getParamNames", parm, result, payload, false)) { return(ret); } if (result.Count != 3 || result[0].GetInt() != 1 || result[2].Type != XmlRpcType.Array) { ROS.Warn()("Expected a return code, a description, and a list!"); return(ret); } for (int i = 0; i < payload.Count; i++) { ret.Add(payload[i].GetString()); } return(ret); }
private SimTime() { new Thread(() => { try { while (!ROS.IsStarted() && !ROS.ShuttingDown) { Thread.Sleep(100); } if (!ROS.ShuttingDown) { nodeHandle = new NodeHandle(); simTimeSubscriber = nodeHandle.Subscribe <Clock>("/clock", 1, SimTimeCallback); } } catch (Exception e) { logger.LogError(e, "Caught exception in sim time thread: " + e.Message); } }).Start(); }
public void RemoveById(long ownerId) { SetupTls(); IDInfo idinfo; lock ( idInfoMutex ) { if (!idInfo.ContainsKey(ownerId)) { return; } idinfo = idInfo[ownerId]; } if (idinfo.id == tls.calling_in_this_thread) { RemoveAll(ownerId); } else { ROS.Debug()($"[{ThisNode.Name}] removeByID w/ WRONG THREAD ID"); RemoveAll(ownerId); } }
public void Spin(CancellationToken token) { TimeSpan wallDuration = new TimeSpan(0, 0, 0, 0, ROS.WallDuration); ROS.Info()($"[{ThisNode.Name}] Start spinning"); while (ROS.ok) { DateTime begin = DateTime.UtcNow; callbackQueue.CallAvailable(ROS.WallDuration); if (token.IsCancellationRequested) { break; } DateTime end = DateTime.UtcNow; var remainingTime = wallDuration - (end - begin); if (remainingTime > TimeSpan.Zero) { Thread.Sleep(remainingTime); } } }
private bool onHeaderReceived(Connection conn, Header header) { string md5sum; if (header.Values.ContainsKey("md5sum")) { md5sum = header.Values["md5sum"]; } else { ROS.Error()($"[{ThisNode.Name}] 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); }