public bool Parse(byte[] buffer, int size, ref string error_msg) { int i = 0; while (i < size) { int thispiece = BitConverter.ToInt32(buffer, i); i += 4; byte[] line = new byte[thispiece]; Array.Copy(buffer, i, line, 0, thispiece); string thisheader = Encoding.ASCII.GetString(line); string[] chunks = thisheader.Split('='); if (chunks.Length != 2) { i += thispiece; continue; } Values[chunks[0].Trim()] = chunks[1].Trim(); i += thispiece; } if (i != size) { ROS.Warn()($"[{ThisNode.Name}] Could not parse connection header."); return(false); } return(true); }
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); }
public bool validateHeader(Header header, ref string error_message) { string md5sum = "", topic = "", client_callerid = ""; if (!header.Values.ContainsKey("md5sum") || !header.Values.ContainsKey("topic") || !header.Values.ContainsKey("callerid")) { string msg = $"[{ThisNode.Name}] Header from subscriber did not have the required elements: md5sum, topic, callerid"; ROS.Warn()(msg); error_message = msg; return(false); } md5sum = (string)header.Values["md5sum"]; topic = (string)header.Values["topic"]; client_callerid = (string)header.Values["callerid"]; if (Dropped) { string msg = $"[{ThisNode.Name}] Received a tcpros connection for a nonexistent topic [{topic}] from [{client_callerid}]."; ROS.Warn()(msg); error_message = msg; return(false); } if (Md5sum != md5sum && (md5sum != "*") && Md5sum != "*") { string datatype = header.Values.ContainsKey("type") ? (string)header.Values["type"] : "unknown"; string msg = $"[{ThisNode.Name}] Client [{client_callerid}] wants topic [{topic}] to have datatype/md5sum [{datatype}/{md5sum}], but our version has [{DataType}/{Md5sum}]. Dropping connection"; ROS.Warn()(msg); error_message = msg; return(false); } return(true); }
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); }
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 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); }
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); }
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 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); }
/// <summary> /// Checks if the given topic is valid. /// </summary> /// <typeparam name="T">Advertise Options </typeparam> /// <param name="ops"></param> /// <returns></returns> private bool isValid <T>(AdvertiseOptions <T> ops) where T : RosMessage, new() { if (ops.dataType == "*") { throw new Exception("Advertising with * as the datatype is not allowed. Topic [" + ops.topic + "]"); } if (ops.md5Sum == "*") { throw new Exception("Advertising with * as the md5sum is not allowed. Topic [" + ops.topic + "]"); } if (ops.md5Sum == "") { throw new Exception("Advertising on topic [" + ops.topic + "] with an empty md5sum"); } if (ops.dataType == "") { throw new Exception("Advertising on topic [" + ops.topic + "] with an empty datatype"); } if (string.IsNullOrEmpty(ops.messageDefinition)) { ROS.Warn()($"[{ThisNode.Name}] Advertising on topic [{ops.topic}] with an empty message definition. Some tools may not work correctly"); } return(true); }
public bool OnConnectionHeaderReceived(Connection conn, Header header) { bool ret = false; if (header.Values.ContainsKey("topic")) { TransportSubscriberLink sub_link = new TransportSubscriberLink(); ret = sub_link.Initialize(conn); ret &= sub_link.HandleHeader(header); } else if (header.Values.ContainsKey("service")) { IServiceClientLink iscl = new IServiceClientLink(); ret = iscl.initialize(conn); ret &= iscl.handleHeader(header); } else { ROS.Warn()($"[{ThisNode.Name}] Got a connection for a type other than topic or service from [{conn.RemoteString}]."); return(false); } //ROS.Debug()( $"[{ThisNode.Name}] CONNECTED [{val}]. WIN." ); return(ret); }
internal bool AdvertiseService <MReq, MRes>(AdvertiseServiceOptions <MReq, MRes> ops) where MReq : RosMessage, new() where MRes : RosMessage, new() { lock ( shuttingDownMutex ) { if (shuttingDown) { return(false); } } lock ( servicePublicationsMutex ) { if (IsServiceAdvertised(ops.service)) { ROS.Warn()($"[{ThisNode.Name}] Tried to advertise a service that is already advertised in this node [{ops.service}]"); return(false); } if (ops.helper == null) { ops.helper = new ServiceCallbackHelper <MReq, MRes>(ops.srv_func); } ServicePublication <MReq, MRes> pub = new ServicePublication <MReq, MRes>(ops.service, ops.md5sum, ops.datatype, ops.req_datatype, ops.res_datatype, ops.helper, ops.callback_queue, ops.tracked_object); servicePublications.Add(pub); } XmlRpcValue args = new XmlRpcValue(), result = new XmlRpcValue(), payload = new XmlRpcValue(); args.Set(0, ThisNode.Name); args.Set(1, ops.service); args.Set(2, string.Format("rosrpc://{0}:{1}", Network.host, connectionManager.TCPPort)); args.Set(3, xmlrpcManager.Uri); if (!Master.execute("registerService", args, result, payload, true)) { throw new RosException("RPC \"registerService\" for service " + ops.service + " failed."); } return(true); }
private void PendingConnectionDone(PendingConnection conn, Task <XmlRpcCallResult> callTask) { lock ( pendingConnections ) { pendingConnections.Remove(conn); } if (callTask.IsFaulted) { List <string> errorMessages = new List <string>(); foreach (Exception exception in callTask.Exception.InnerExceptions) { errorMessages.Add(BuildExceptionMessages(exception)); } ROS.Warn()($"[{ThisNode.Name}] Negotiating for {name} has failed (Error: {string.Join( ", ", errorMessages.ToArray() )})."); return; } if (!callTask.Result.Success) { ROS.Warn()($"[{ThisNode.Name}] Negotiating for {name} has failed. XML-RPC call failed."); return; } var resultValue = callTask.Result.Value; lock ( shutdown_mutex ) { if (shutting_down || _dropped) { return; } } var proto = new XmlRpcValue(); if (!XmlRpcManager.Instance.ValidateXmlRpcResponse("requestTopic", resultValue, proto)) { ROS.Warn()($"[{ThisNode.Name}] Negotiating for {name} has failed."); return; } string peerHost = conn.Client.Host; int peerPort = conn.Client.Port; string xmlrpcUri = $"http://{peerHost}:{peerPort}/"; if (proto.Count == 0) { ROS.Debug()($"[{ThisNode.Name}] Could not agree on any common protocols with [{xmlrpcUri}] for topic [{name}]"); return; } if (proto.Type != XmlRpcType.Array) { ROS.Warn()($"[{ThisNode.Name}] Available protocol info returned from {xmlrpcUri} is not a list."); return; } string protoName = proto[0].GetString(); if (protoName == "UDPROS") { ROS.Error()($"[{ThisNode.Name}] UDP is currently not supported. Use TCPROS instead."); } else if (protoName == "TCPROS") { if (proto.Count != 3 || proto[1].Type != XmlRpcType.String || proto[2].Type != XmlRpcType.Int) { ROS.Warn()($"[{ThisNode.Name}] TcpRos Publisher should implement string, int as parameter"); return; } string pubHost = proto[1].GetString(); int pubPort = proto[2].GetInt(); ROS.Debug()($"[{ThisNode.Name}] Connecting via tcpros to topic [{name}] at host [{pubHost}:{pubPort}]"); var transport = new TcpTransport(PollManager.Instance.poll_set) { _topic = name }; if (transport.connect(pubHost, pubPort)) { var connection = new Connection(); var pubLink = new TransportPublisherLink(this, xmlrpcUri); connection.initialize(transport, false, null); pubLink.initialize(connection); ConnectionManager.Instance.AddConnection(connection); lock ( publisher_links_mutex ) { addPublisherLink(pubLink); } ROS.Debug()($"[{ThisNode.Name}] Connected to publisher of topic [{name}] at [{pubHost}:{pubPort}]"); } else { ROS.Error()($"[{ThisNode.Name}] Failed to connect to publisher of topic [{name}] at [{pubHost}:{pubPort}]"); } } else { ROS.Error()($"[{ThisNode.Name}] The XmlRpc Server does not provide a supported protocol."); } }
public bool pubUpdate(IEnumerable <string> publisherUris) { lock ( shutdown_mutex ) { if (shutting_down || _dropped) { return(false); } } bool retval = true; ROS.Debug()($"[{ThisNode.Name}] Publisher update for [{name}]"); var additions = new List <string>(); List <PublisherLink> subtractions; lock ( publisher_links_mutex ) { subtractions = publisher_links.Where(x => !publisherUris.Any(u => urisEqual(x.XmlRpcUri, u))).ToList(); foreach (string uri in publisherUris) { bool found = publisher_links.Any(spc => urisEqual(uri, spc.XmlRpcUri)); if (found) { continue; } lock ( pendingConnections ) { if (pendingConnections.Any(pc => urisEqual(uri, pc.RemoteUri))) { found = true; } if (!found) { additions.Add(uri); } } } } foreach (PublisherLink link in subtractions) { if (link.XmlRpcUri != XmlRpcManager.Instance.Uri) { ROS.Debug()($"[{ThisNode.Name}] Disconnecting from publisher [{link.CallerID}] of topic [{name}] at [{link.XmlRpcUri}]"); link.drop(); } else { ROS.Warn()($"[{ThisNode.Name}] Cannot disconnect from self for topic: {name}"); } } foreach (string i in additions) { ROS.Debug()($"[{ThisNode.Name}] Start connecting to {i} for [{name}]"); if (XmlRpcManager.Instance.Uri != i) { retval &= NegotiateConnection(i); //ROS.Debug()("NEGOTIATINGING"); } else { ROS.Info()($"[{ThisNode.Name}] Skipping myself ({name}, {XmlRpcManager.Instance.Uri})"); } } return(retval); }
/// <summary> /// Execute a remote procedure call on the ROS master. /// </summary> /// <param name="method"></param> /// <param name="request">Full request to send to the master </param> /// <param name="waitForMaster">If you recieve an unseccessful status code, keep retrying.</param> /// <param name="response">Full response including status code and status message. Initially empty.</param> /// <param name="payload">Location to store the actual data requested, if any.</param> /// <returns></returns> public static async Task <bool> ExecuteAsync(string method, XmlRpcValue request, XmlRpcValue response, XmlRpcValue payload, bool waitForMaster) { bool supprressWarning = false; var startTime = DateTime.UtcNow; try { var client = new XmlRpcClient(host, port); while (true) { // check if we are shutting down if (XmlRpcManager.Instance.IsShuttingDown) { return(false); } try { var result = await client.ExecuteAsync(method, request); // execute the RPC call response.Set(result.Value); if (result.Success) { // validateXmlrpcResponse logs error in case of validation error // So we don't need any logging here. if (XmlRpcManager.Instance.ValidateXmlRpcResponse(method, result.Value, payload)) { return(true); } else { return(false); } } else { if (response.IsArray && response.Count >= 2) { ROS.Error()($"[{ThisNode.Name}] Execute failed: return={response[0].GetInt()}, desc={response[1].GetString()}"); } else { ROS.Error()($"[{ThisNode.Name}] response type: {response.Type.ToString()}"); } } } catch (Exception ex) { // no connection to ROS Master if (waitForMaster) { if (!supprressWarning) { ROS.Warn()($"[{ThisNode.Name}] [{method}] Could not connect to master at [{host}:{port}]. Retrying for the next {retryTimeout.TotalSeconds} seconds."); supprressWarning = true; } // timeout expired, throw exception if (retryTimeout.TotalSeconds > 0 && DateTime.UtcNow.Subtract(startTime) > retryTimeout) { ROS.Error()($"[{ThisNode.Name}] [{method}] Timed out trying to connect to the master [{host}:{port}] after [{retryTimeout.TotalSeconds}] seconds"); throw new RosException($"Cannot connect to ROS Master at {host}:{port}", ex); } } else { throw new RosException($"Cannot connect to ROS Master at {host}:{port}", ex); } } await Task.Delay(250); // recreate the client and reinitiate master connection client = new XmlRpcClient(host, port); } } catch (ArgumentNullException e) { ROS.Error()($"[{ThisNode.Name}] {e.ToString()}"); } ROS.Error()($"[{ThisNode.Name}] Master API call: {method} failed!\n\tRequest:\n{request}"); return(false); }