internal bool LookupService(string name, ref string serv_host, ref int serv_port) { XmlRpcValue args = new XmlRpcValue(), result = new XmlRpcValue(), payload = new XmlRpcValue(); args.Set(0, ThisNode.Name); args.Set(1, name); if (!Master.execute("lookupService", args, result, payload, false)) { ROS.Warn()($"[{ThisNode.Name}] Service [{name}]: Not available at ROS master"); return(false); } string serv_uri = payload.GetString(); if (serv_uri.Length == 0) { ROS.Error()($"[{ThisNode.Name}] Service [{name}]: Empty server URI returned from master"); return(false); } if (!Network.SplitUri(serv_uri, out serv_host, out serv_port)) { ROS.Error()($"[{ThisNode.Name}] Service [{name}]: Bad service uri [{serv_uri}]"); return(false); } return(true); }
private void onConnectionDropped(Connection conn, Connection.DropReason reason) { ROS.Debug()($"[{ThisNode.Name}] TransportPublisherLink: onConnectionDropped from remote {conn.RemoteString} with reason {reason.ToString()}"); if (dropping || conn != connection) { return; } if (reason == Connection.DropReason.TransportDisconnect) { needs_retry = true; next_retry = DateTime.UtcNow.Add(retry_period); if (retry_timer == null) { retry_timer = ROS.timerManager.StartTimer(onRetryTimer, 100); } else { retry_timer.Restart(); } } else { if (reason == Connection.DropReason.HeaderError) { ROS.Error()($"[{ThisNode.Name}] Error in the Header: {( parent != null ? parent.name : "unknown" )}"); } drop(); } }
protected bool precall(string service_md5sum) { if (service_md5sum != md5sum) { ROS.Error()($"[{ThisNode.Name}] Call to service [{service} with md5sum [{service_md5sum} does not match md5sum when the handle was created([{md5sum}])"); return(false); } if (server_link != null && server_link.connection.dropped) { if (persistent) { ROS.Warn()($"[{ThisNode.Name}] Persistent service client's server link has been dropped. Trying to reconnect to proceed with this call"); } server_link = null; } if (is_shutdown && persistent) { ROS.Warn()($"[{ThisNode.Name}] Persistent service client is self-resurrecting"); } is_shutdown = false; if (persistent && server_link == null || !persistent) { server_link = linkmaker(); } return(true); }
private bool onMessageLength(Connection conn, byte[] buffer, int size, bool success) { if (retry_timer != null) { ROS.timerManager.RemoveTimer(ref retry_timer); } if (!success) { if (connection != null) { connection.read(4, onMessageLength); } return(true); } if (conn != connection || size != 4) { return(false); } int len = BitConverter.ToInt32(buffer, 0); int lengthLimit = 1000000000; if (len > lengthLimit) { ROS.Error()($"[{ThisNode.Name}] TransportPublisherLink length exceeds limit of {lengthLimit}. Dropping connection"); drop(); return(false); } connection.read(len, onMessage); return(true); }
private void socketUpdate(int events) { lock ( closeMutex ) { if (closed) { return; } } if (isServer) { TcpTransport transport = accept(); if (transport != null) { if (accept_cb == null) { throw new NullReferenceException("Accept callback is null"); } accept_cb(transport); } } else { if ((events & POLLIN) != 0 && expectingRead) //POLL IN FLAG { if (read_cb != null) { read_cb(this); } } if ((events & POLLOUT) != 0 && expectingWrite) { if (write_cb != null) { write_cb(this); } } if ((events & POLLERR) != 0 || (events & POLLHUP) != 0 || (events & POLLNVAL) != 0) { int error = 0; try { error = (int)socket.GetSocketOption(SocketOptionLevel.Socket, SocketOptionName.Error); } catch (Exception e) { ROS.Error()($"[{ThisNode.Name}] Failed to get sock options! (error: {error}){e}"); } if (error != 0) { ROS.Error()($"[{ThisNode.Name}] Socket error = {error}"); } close(); } } }
internal override CallResult Call() { if (link.connection.dropped) { return(CallResult.Invalid); } ServiceCallbackHelperParams <MReq, MRes> parms = new ServiceCallbackHelperParams <MReq, MRes> { request = new MReq(), response = new MRes(), connection_header = link.connection.header.Values }; parms.request.Deserialize(buffer); try { bool ok = isp.helper.call(parms); link.processResponse(parms.response, ok); } catch (Exception e) { string str = $"[{ThisNode.Name}] Exception thrown while processing service call: {e}"; ROS.Error()(str); link.processResponse(str, false); return(CallResult.Invalid); } return(CallResult.Success); }
public bool handleHeader(Header header) { if (!header.Values.ContainsKey("md5sum") || !header.Values.ContainsKey("service") || !header.Values.ContainsKey("callerid")) { string bbq = $"[{ThisNode.Name}] Error in TcpRos header. Required elements (md5sum, service, callerid) are missing"; ROS.Error()(bbq); connection.sendHeaderError(ref bbq); return(false); } string md5sum = (string)header.Values["md5sum"]; string service = (string)header.Values["service"]; string client_callerid = (string)header.Values["callerid"]; if (header.Values.ContainsKey("persistent") && ((string)header.Values["persistent"] == "1" || (string)header.Values["persistent"] == "true")) { persistent = true; } ROS.Debug()($"[{ThisNode.Name}] Service client [{client_callerid}] wants service [{service}] with md5sum [{md5sum}]"); IServicePublication isp = ServiceManager.Instance.LookupServicePublication(service); if (isp == null) { string bbq = $"[{ThisNode.Name}] Received a TcpRos connection for a nonexistent service [{service}]"; ROS.Warn()(bbq); connection.sendHeaderError(ref bbq); return(false); } if (isp.md5sum != md5sum && md5sum != "*" && isp.md5sum != "*") { string bbq = $"[{ThisNode.Name}] Client wants service {service} to have md5sum {md5sum} but it has {isp.md5sum}. Dropping connection"; ROS.Error()(bbq); connection.sendHeaderError(ref bbq); return(false); } if (isp.isDropped) { string bbq = $"[{ThisNode.Name}] Received a TcpRos connection for a nonexistent service [{service}]"; ROS.Warn()(bbq); connection.sendHeaderError(ref bbq); return(false); } parent = isp; IDictionary <string, string> m = new Dictionary <string, string>(); m["request_type"] = isp.req_datatype; m["response_type"] = isp.res_datatype; m["type"] = isp.datatype; m["md5sum"] = isp.md5sum; m["callerid"] = ThisNode.Name; connection.writeHeader(m, onHeaderWritten); isp.addServiceClientLink(this); return(true); }
public void setNoDelay(bool nd) { try { socket.NoDelay = nd; } catch (Exception e) { ROS.Error()($"[{ThisNode.Name}] {e.ToString()}"); } }
public void SendEvent(RosMessage msg) { if (Event != null) { Event(msg); } else { ROS.Error()($"[{ThisNode.Name}] {nameof( Event )} is null"); } }
private async Task RunSendLoopAsync(Header header) { await Task.Yield(); try { // header handshake try { await HandleHeader(header).ConfigureAwait(false); } catch (Exception e) { if (ROS.shuttingDown) { await connection.SendHeaderError("ROS node shutting down", cancel).ConfigureAwait(false); } else { logger.LogWarning(e, e.Message); await connection.SendHeaderError(e.Message, cancel).ConfigureAwait(false); } connection.Close(50); throw; } // read messages from queue and send them while (await outbox.MoveNext(cancel).ConfigureAwait(false)) { cancel.ThrowIfCancellationRequested(); var current = outbox.Current; try { await WriteMessage(current).ConfigureAwait(false); } catch (Exception ex) { // Catch exceptions and log them instead of just ignoring them... ROS.Error()("Error while writing message, not sending. Error: {0}, Stacktrace: {1}", ex.ToString(), ex.StackTrace); throw; } } } finally { UnregisterSubscriberLink(); } }
private void requestTopicCallback(XmlRpcValue parm, XmlRpcValue res) { //XmlRpcValue res = XmlRpcValue.Create(ref result) // , parm = XmlRpcValue.Create(ref parms); //result = res.instance; if (!requestTopic(parm[1].GetString(), parm[2], ref res)) { string error = $"[{ThisNode.Name}] Unknown error while handling XmlRpc call to requestTopic"; ROS.Error()(error); XmlRpcManager.ResponseInt(0, error, 0)(res); } }
private bool onResponseOkAndLength(Connection conn, byte[] buf, int size, bool success) { if (conn != connection) { throw new ArgumentException("Unknown connection", nameof(conn)); } if (size != 5) { throw new ArgumentException($"Wrong size {size}", nameof(size)); } if (!success) { return(false); } byte ok = buf[0]; int len = BitConverter.ToInt32(buf, 1); int lengthLimit = 1000000000; if (len > lengthLimit) { ROS.Error()($"[{ThisNode.Name}] Message length exceeds limit of {lengthLimit}. Dropping connection."); 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) { ROS.Debug()($"[{ThisNode.Name}] Reading message with length of {len}."); connection.read(len, onResponse); } else { byte[] f = new byte[0]; onResponse(conn, f, 0, true); } return(true); }
public bool call(RosMessage req, ref RosMessage resp) { if (resp == null) { //instantiate null response IN CASE this call succeeds resp = RosMessage.Generate(req.MessageType.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) { ROS.Debug()($"[{ThisNode.Name}] info.finished_condition.WaitOne();"); info.finished_condition.WaitOne(); } if (info.success) { // response is only sent on success => don't try to deserialize on failure. resp.Deserialize(resp.Serialized); } if (!string.IsNullOrEmpty(info.exception)) { ROS.Error()($"[{ThisNode.Name}] Service call failed: service [{name}] responded with an error: {info.exception}"); } return(info.success); }
public TcpTransport(PollSet pollset, int flags) { if (pollset != null) { pollSet = pollset; pollSet.DisposingEvent += close; } else { ROS.Error()($"[{ThisNode.Name}] Null pollset in tcptransport ctor"); } this.flags = flags; }
public bool HandleHeader(Header header) { if (!header.Values.ContainsKey("topic")) { string msg = $"[{ThisNode.Name}] Header from subscriber did not have the required element: topic"; ROS.Warn()(msg); connection.sendHeaderError(ref msg); return(false); } string name = (string)header.Values["topic"]; string client_callerid = (string)header.Values["callerid"]; Publication pt = TopicManager.Instance.lookupPublication(name); if (pt == null) { string msg = $"[{ThisNode.Name}] received a connection for a nonexistent topic [{name}] from [{connection.transport}] [{client_callerid}]"; ROS.Warn()(msg); connection.sendHeaderError(ref msg); return(false); } string error_message = ""; if (!pt.validateHeader(header, ref error_message)) { connection.sendHeaderError(ref error_message); ROS.Error()($"[{ThisNode.Name}] {error_message}"); return(false); } destination_caller_id = client_callerid; connection_id = ConnectionManager.Instance.GetNewConnectionId(); name = pt.Name; topic = name; parent = pt; lock ( parent ) { maxQueue = parent.MaxQueue; } var m = new Dictionary <string, string>(); m["type"] = pt.DataType; m["md5sum"] = pt.Md5sum; m["message_definition"] = pt.MessageDefinition; m["callerid"] = ThisNode.Name; m["latching"] = Convert.ToString(pt.Latch); connection.writeHeader(m, OnHeaderWritten); pt.addSubscriberLink(this); ROS.Debug()($"[{ThisNode.Name}] Finalize transport subscriber link for {name}"); return(true); }
private async Task ProcessCall(CallInfo call) { RosMessage request = call.Request; // serialize and send request request.Serialized = request.Serialize(); await connection.WriteBlock(BitConverter.GetBytes(request.Serialized.Length), 0, 4, cancel).ConfigureAwait(false); await connection.WriteBlock(request.Serialized, 0, request.Serialized.Length, cancel).ConfigureAwait(false); // read response header var receiveBuffer = await connection.ReadBlock(5, cancel).ConfigureAwait(false); bool success = receiveBuffer[0] != 0; int responseLength = BitConverter.ToInt32(receiveBuffer, 1); if (responseLength < 0 || responseLength > Connection.MESSAGE_SIZE_LIMIT) { var errorMessage = $"Message length exceeds limit of {Connection.MESSAGE_SIZE_LIMIT}. Dropping connection."; ROS.Error()(errorMessage); throw new ConnectionError(errorMessage); } if (responseLength > 0) { logger.LogDebug($"Reading message with length of {responseLength}."); receiveBuffer = await connection.ReadBlock(responseLength, cancel).ConfigureAwait(false); } else { receiveBuffer = new byte[0]; } if (success) { call.Response.Serialized = receiveBuffer; call.Tcs.TrySetResult(true); } else { if (receiveBuffer.Length > 0) { // call failed with reason call.Tcs.TrySetException(new Exception(Encoding.UTF8.GetString(receiveBuffer))); } call.Tcs.TrySetResult(false); } }
public bool registerSubscriber(Subscription s, string datatype) { string uri = XmlRpcManager.Instance.Uri; var args = new XmlRpcValue(ThisNode.Name, s.name, datatype, uri); var result = new XmlRpcValue(); var payload = new XmlRpcValue(); if (!Master.execute("registerSubscriber", args, result, payload, true)) { ROS.Error()($"[{ThisNode.Name}] RPC \"registerSubscriber\" for service {s.name} failed."); return(false); } var pub_uris = new List <string>(); for (int i = 0; i < payload.Count; i++) { XmlRpcValue load = payload[i]; string pubed = load.GetString(); if (pubed != uri && !pub_uris.Contains(pubed)) { pub_uris.Add(pubed); } } bool self_subscribed = false; Publication pub = null; string sub_md5sum = s.md5sum; lock ( advertisedTopicsMutex ) { foreach (Publication p in advertisedTopics) { pub = p; string pub_md5sum = pub.Md5sum; if (pub.Name == s.name && md5sumsMatch(pub_md5sum, sub_md5sum) && !pub.Dropped) { self_subscribed = true; break; } } } s.pubUpdate(pub_uris); if (self_subscribed) { s.addLocalConnection(pub); } return(true); }
public static void Init(string name, IDictionary <string, string> remappings, int options) { Name = name; bool disableAnonymous = false; if (remappings.ContainsKey("__name")) { Name = remappings["__name"]; disableAnonymous = true; } if (remappings.ContainsKey("__ns")) { Namespace = remappings["__ns"]; } if (string.IsNullOrEmpty(Namespace)) { Namespace = "/"; } long walltime = DateTime.UtcNow.Subtract(Process.GetCurrentProcess().StartTime).Ticks; Names.Init(remappings); if (Name.Contains("/")) { throw new ArgumentException("Slashes '/' are not allowed in names", nameof(name)); } if (Name.Contains("~")) { throw new ArgumentException("Tildes '~' are not allowed in names", nameof(name)); } try { Name = Names.Resolve(Namespace, Name); } catch (Exception e) { ROS.Error()($"[{Name}] {e.ToString()}"); } if ((options & (int)InitOption.AnonymousName) == (int)InitOption.AnonymousName && !disableAnonymous) { int lbefore = Name.Length; Name += "_" + walltime; if (Name.Length - lbefore > 201) { Name = Name.Remove(lbefore + 201); } } }
private CallOneResult CallOne(TLS tls) { CallbackInfo info = tls.Head; if (info == null) { return(CallOneResult.Empty); } IDInfo idinfo = null; idinfo = GetIdInfo(info.RemovalId); if (idinfo != null) { CallbackInterface cb = info.Callback; lock (idinfo.calling_rw_mutex) { CallbackInterface.CallResult result = CallbackInterface.CallResult.Invalid; tls.SpliceOut(info); if (!info.MarkedForRemoval) { try { result = cb.Call(); } catch (Exception ex) { ROS.Error()($"[{ThisNode.Name}] Error during callback. Error: {ex.ToString()}, Stacktrace: {ex.StackTrace}"); } } if (result == CallbackInterface.CallResult.TryAgain && !info.MarkedForRemoval) { lock ( mutex ) { callbacks.Add(info); count++; } return(CallOneResult.TryAgain); } } return(CallOneResult.Called); } CallbackInfo cbi = tls.SpliceOut(info); if (cbi != null) { cbi.Callback.Call(); } return(CallOneResult.Called); }
public void Shutdown() { if (thread != null && !shutting_down) { shutting_down = true; poll_set.Dispose(); poll_set = null; signals.Clear(); if (!thread.Join(2000)) { ROS.Error()($"[{ThisNode.Name}] thread.Join() timed out."); } thread = null; } }
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.ConfigureAwait(false); if (success) { // response is only sent on success response.Deserialize(response.Serialized); } return(success, response); } catch (Exception e) { string message = $"Service call failed: service [{name}] responded with an error: {e.Message}"; ROS.Error()(message); return(false, null); } finally { if (!persistent) { queue.OnCompleted(); } } }
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); }
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); }
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); }
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; } } } }
/// <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 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); } }
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); } }
/// <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}"); } }