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