public static async Task <bool> WaitForService(string serviceName, TimeSpan timeout)
{
DateTime startTime = DateTime.UtcNow;
bool printed = false;
while (ROS.OK)
{
if (await Exists(serviceName, !printed))
{
break;
}
printed = true;
if (timeout >= TimeSpan.Zero)
{
if (DateTime.UtcNow - startTime > timeout)
{
return(false);
}
}
await Task.Delay(ROS.WallDuration);
}
if (printed && ROS.OK)
{
string mappedName = Names.Resolve(serviceName);
ROS.Info()("waitForService: Service[{0}] is now available.", mappedName);
}
return(true);
}
public bool pubUpdate(string topic, List <string> pubs)
{
ROS.Debug()($"[{ThisNode.Name}] TopicManager is updating publishers for {topic}");
Subscription sub = null;
lock ( subcriptionsMutex )
{
if (shuttingDown)
{
return(false);
}
foreach (Subscription s in subscriptions)
{
if (s.name != topic || s.IsDropped)
{
continue;
}
sub = s;
break;
}
}
if (sub != null)
{
return(sub.pubUpdate(pubs));
}
ROS.Info()($"[{ThisNode.Name}] Request for updating publishers of topic {topic}, which has no subscribers.");
return(false);
}
public static bool waitForService(string serviceName, TimeSpan timeout)
{
string mapped_name = Names.Resolve(serviceName);
DateTime start_time = DateTime.UtcNow;
bool printed = false;
while (ROS.ok)
{
if (exists(serviceName, !printed))
{
break;
}
printed = true;
if (timeout >= TimeSpan.Zero)
{
if (DateTime.UtcNow.Subtract(start_time) > timeout)
{
return(false);
}
}
Thread.Sleep(ROS.WallDuration);
}
if (printed && ROS.ok)
{
ROS.Info()($"[{ThisNode.Name}] waitForService: Service[{mapped_name}] is now available.");
}
return(true);
}
public static async Task <bool> Exists(string serviceName, bool logFailureReason = false)
{
string mappedName = Names.Resolve(serviceName);
string host;
int port;
try
{
(host, port) = await ServiceManager.Instance.LookupServiceAsync(mappedName);
}
catch
{
if (logFailureReason)
{
ROS.Info()("waitForService: Service[{0}] has not been advertised, waiting...", mappedName);
}
return(false);
}
using (var tcpClient = new TcpClient())
{
try
{
await tcpClient.ConnectAsync(host, port);
}
catch
{
if (logFailureReason)
{
ROS.Info()("waitForService: Service[{0}] could not connect to host [{1}:{2}], waiting...", mappedName, host, port);
}
return(false);
}
var headerFields = new Dictionary <string, string>
{
{ "probe", "1" },
{ "md5sum", "*" },
{ "callerid", ThisNode.Name },
{ "service", mappedName }
};
Header.Write(headerFields, out byte[] headerbuf, out int size);
byte[] sizebuf = BitConverter.GetBytes(size);
var stream = tcpClient.GetStream();
await stream.WriteAsync(sizebuf, 0, sizebuf.Length);
await stream.WriteAsync(headerbuf, 0, size);
}
return(true);
}
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);
}
}
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);
}
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 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 onRequestWritten(Connection conn)
{
ROS.Info()($"[{ThisNode.Name}] onRequestWritten(Connection conn)");
connection.read(5, onResponseOkAndLength);
return(true);
}
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);
}
public bool connect(string host, int port)
{
socket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
connectedHost = host;
connectedPort = port;
if (!setNonBlocking())
{
throw new Exception($"[{ThisNode.Name}] Failed to make socket nonblocking");
}
setNoDelay(true);
IPAddress ip;
if (!IPAddress.TryParse(host, out ip))
{
ip = Dns.GetHostAddressesAsync(host).Result.Where(x => !x.ToString().Contains(":")).FirstOrDefault();
if (ip == null)
{
close();
ROS.Error()($"[{ThisNode.Name}] Couldn't resolve host name [{host}]");
return(false);
}
}
if (ip == null)
{
return(false);
}
IPEndPoint ipep = new IPEndPoint(ip, port);
LocalEndPoint = ipep;
DateTime connectionAttempted = DateTime.UtcNow;
IAsyncResult asyncres = socket.BeginConnect(ipep, iar =>
{
lock (this)
{
if (socket != null)
{
try
{
socket.EndConnect(iar);
}
catch (Exception e)
{
ROS.Error()($"[{ThisNode.Name}] {e.ToString()}");
}
}
}
}, null);
bool completed = false;
while (ROS.ok && !ROS.shutting_down)
{
completed = asyncres.AsyncWaitHandle.WaitOne(10);
if (completed)
{
break;
}
if (DateTime.UtcNow.Subtract(connectionAttempted).TotalSeconds >= 3)
{
ROS.Info()($"[{ThisNode.Name}] Trying to connect for {DateTime.UtcNow.Subtract( connectionAttempted ).TotalSeconds}s\t: {this}");
if (!asyncres.AsyncWaitHandle.WaitOne(100))
{
socket.Close();
socket = null;
}
}
}
if (!completed || socket == null || !socket.Connected)
{
return(false);
}
else
{
ROS.Debug()($"[{ThisNode.Name}] TcpTransport connection established.");
}
return(ROS.ok && initializeSocket());
}
