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;
if (success)
{
// response is only sent on success
response.Deserialize(response.Serialized);
}
return(success, response);
}
catch (Exception e)
{
ROS.Error()($"Service call failed: service [{name}] responded with an error: {e}");
return(false, null);
}
finally
{
if (!persistent)
{
queue.OnCompleted();
}
}
}
private void RemoveDroppedConnections()
{
Connection[] localDroppedConnections = null;
lock ( dropped_connections_mutex )
{
localDroppedConnections = dropped_connections.ToArray();
dropped_connections.Clear();
}
lock ( connections_mutex )
{
foreach (Connection c in localDroppedConnections)
{
ROS.Debug()($"[{ThisNode.Name}] Removing dropped connection: {c.CallerID}");
connections.Remove(c);
}
}
}
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo)
{
var logMessage = new Log
{
msg = message,
name = ThisNode.Name,
file = callerInfo.FilePath,
function = callerInfo.MemberName,
line = (uint)callerInfo.LineNumber,
level = (byte)level,
header = new Messages.std_msgs.Header {
stamp = ROS.GetTime()
}
};
logMessage.topics = topicManager.GetAdvertisedTopics();
queue.TryOnNext(logMessage);
}
private void ThreadFunc()
{
TimeSpan wallDuration = new TimeSpan(0, 0, 0, 0, ROS.WallDuration);
while (ROS.ok)
{
DateTime begin = DateTime.UtcNow;
CallAvailable(ROS.WallDuration);
DateTime end = DateTime.UtcNow;
var remainingTime = wallDuration - (end - begin);
if (remainingTime > TimeSpan.Zero)
{
Thread.Sleep(remainingTime);
}
}
ROS.Debug()($"[{ThisNode.Name}] CallbackQueue thread broke out!");
}
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;
}
}
}
}
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);
}
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);
}
/// <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 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);
}
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);
}
}
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);
}
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);
}
}
async Task HandleConnection()
{
try
{
await Handshake().ConfigureAwait(false);
while (await callQueue.MoveNext(cancel).ConfigureAwait(false))
{
var call = callQueue.Current;
await ProcessCall(call).ConfigureAwait(false);
}
connection.Socket.Shutdown(SocketShutdown.Send);
connection.Close(50);
}
catch (System.IO.EndOfStreamException ex)
{
ROS.Debug()("EndOfStreamException during connection handling. Message: {0}, Stacktrace : {1}",
ex.ToString(), ex.StackTrace);
}
catch (System.IO.IOException ex)
{
ROS.Debug()("IOException during connection handling. Message: {0}, Stacktrace : {1}",
ex.ToString(), ex.StackTrace);
}
catch (Exception e)
{
if (!(e is OperationCanceledException))
{
logger.LogDebug($"Service client for [{name}] dropped: {e.Message}");
}
FlushCallQueue(e);
callQueue = new AsyncQueue <CallInfo>(MAX_CALL_QUEUE_LENGTH, true);
throw;
}
finally
{
logger.LogDebug($"Removing service client for [{name}] from ServiceManager.");
ServiceManager.Instance.RemoveServiceServerLinkAsync(this);
}
}
/// <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}");
}
}
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);
}
}
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo)
{
var logMessage = new Log
{
msg = message,
name = ThisNode.Name,
file = callerInfo.FilePath,
function = callerInfo.MemberName,
line = (uint)callerInfo.LineNumber,
level = ((byte)((int)level)),
header = new Messages.std_msgs.Header()
{
stamp = ROS.GetTime()
}
};
TopicManager.Instance.getAdvertisedTopics(out logMessage.topics);
lock (log_queue)
log_queue.Enqueue(logMessage);
}
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);
}
/// <summary>
/// Creates a publisher with the given advertise options
/// </summary>
/// <typeparam name="M">Type of topic</typeparam>
/// <param name="ops">Advertise options</param>
/// <returns>A publisher with the specified options</returns>
public Publisher <M> advertise <M>(AdvertiseOptions <M> ops) where M : RosMessage, new()
{
ops.topic = resolveName(ops.topic);
if (ops.callbackQueue == null)
{
ops.callbackQueue = Callback;
}
var callbacks = new SubscriberCallbacks(ops.connectCB, ops.disconnectCB, ops.callbackQueue);
if (TopicManager.Instance.advertise(ops, callbacks))
{
var pub = new Publisher <M>(ops.topic, ops.md5Sum, ops.dataType, this, callbacks);
lock ( gate )
{
collection.Publishers.Add(pub);
}
return(pub);
}
ROS.Error()($"[{ThisNode.Name}] Advertisement of publisher has failed");
return(null);
}
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 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 SimTime()
{
new Thread(() =>
{
try
{
while (!ROS.isStarted() && !ROS.shutting_down)
{
Thread.Sleep(100);
}
if (!ROS.shutting_down)
{
nodeHandle = new NodeHandle();
simTimeSubscriber = nodeHandle.subscribe <Clock>("/clock", 1, SimTimeCallback);
}
}
catch (Exception e)
{
ROS.Error()($"[{ThisNode.Name}] Caught exception in sim time thread: {e.Message}");
}
}).Start();
}
private void Construct(string ns, bool validate_name)
{
if (!ROS.initialized)
{
throw new Exception("You must call ROS.Init() before instantiating the first nodehandle");
}
collection = new NodeHandleBackingCollection();
UnresolvedNamespace = ns;
Namespace = validate_name ? ResolveName(ns) : ResolveName(ns, true, true);
OK = true;
lock (gate)
{
if (referenceCount == 0 && !ROS.IsStarted())
{
initializedRos = true;
ROS.Start();
}
++referenceCount;
}
}
private void writeTransport()
{
lock ( writing )
{
if (dropped)
{
return;
}
bool can_write_more = true;
while (write_callback != null && can_write_more && !dropped)
{
int to_write = write_size - write_sent;
int bytes_sent = transport.write(write_buffer, write_sent, to_write);
if (bytes_sent <= 0)
{
return;
}
write_sent += (int)bytes_sent;
if (bytes_sent < write_size - write_sent)
{
can_write_more = false;
}
if (write_sent == write_size && !dropped)
{
WriteFinishedFunc callback = write_callback;
write_callback = null;
write_buffer = null;
write_sent = 0;
write_size = 0;
if (!callback(this))
{
ROS.Error()($"[{ThisNode.Name}] Failed to invoke {callback.GetMethodInfo().Name}");
}
}
}
}
}
public bool initialize(Connection connection)
{
ROS.Debug()($"[{ThisNode.Name}] Init transport publisher link: {parent.name }");
this.connection = connection;
connection.DroppedEvent += onConnectionDropped;
if (connection.transport.getRequiresHeader())
{
connection.setHeaderReceivedCallback(onHeaderReceived);
IDictionary <string, string> header = new Dictionary <string, string>();
header["topic"] = parent.name;
header["md5sum"] = parent.md5sum;
header["callerid"] = ThisNode.Name;
header["type"] = parent.datatype;
header["tcp_nodelay"] = "1";
connection.writeHeader(header, onHeaderWritten);
}
else
{
connection.read(4, onMessageLength);
}
return(true);
}
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);
}
private SimTime()
{
new Thread(() =>
{
try
{
while (!ROS.IsStarted() && !ROS.ShuttingDown)
{
Thread.Sleep(100);
}
if (!ROS.ShuttingDown)
{
nodeHandle = new NodeHandle();
simTimeSubscriber = nodeHandle.Subscribe <Clock>("/clock", 1, SimTimeCallback);
}
}
catch (Exception e)
{
logger.LogError(e, "Caught exception in sim time thread: " + e.Message);
}
}).Start();
}
public void RemoveById(long ownerId)
{
SetupTls();
IDInfo idinfo;
lock ( idInfoMutex )
{
if (!idInfo.ContainsKey(ownerId))
{
return;
}
idinfo = idInfo[ownerId];
}
if (idinfo.id == tls.calling_in_this_thread)
{
RemoveAll(ownerId);
}
else
{
ROS.Debug()($"[{ThisNode.Name}] removeByID w/ WRONG THREAD ID");
RemoveAll(ownerId);
}
}
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 onHeaderReceived(Connection conn, Header header)
{
string md5sum;
if (header.Values.ContainsKey("md5sum"))
{
md5sum = header.Values["md5sum"];
}
else
{
ROS.Error()($"[{ThisNode.Name}] TcpRos header from service server did not have required element: md5sum");
return(false);
}
//TODO check md5sum
bool empty = false;
lock ( call_queue_mutex )
{
empty = (call_queue.Count == 0);
if (empty)
{
header_read = true;
}
}
IsValid = true;
if (!empty)
{
processNextCall();
header_read = true;
}
return(true);
}
