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