private async Task HandleConnection()
{
// establish connection
using (var client = new TcpClient())
{
await client.ConnectAsync(host, port).ConfigureAwait(false);
client.NoDelay = true;
try
{
this.connection = new Connection(client);
// write/read header handshake
await WriteHeader().ConfigureAwait(false);
var headerFields = await connection.ReadHeader(cancel).ConfigureAwait(false);
SetHeader(new Header(headerFields));
// connection established
this.connected = true;
while (!cancel.IsCancellationRequested)
{
// read message length
int length = await connection.ReadInt32(cancel).ConfigureAwait(false);
if (length > Connection.MESSAGE_SIZE_LIMIT)
{
var message = $"Message received in TransportPublisherLink exceeds length limit of {Connection.MESSAGE_SIZE_LIMIT}. Dropping connection";
throw new RosException(message);
}
// read message
var messageBuffer = await connection.ReadBlock(length, cancel).ConfigureAwait(false);
// deserialize message
RosMessage msg = RosMessage.Generate(Parent.DataType);
msg.Serialized = messageBuffer;
msg.connection_header = this.Header.Values;
HandleMessage(msg);
// reset retry delay after first successfully processed message
retryDelay = BASE_RETRY_DELAY;
}
client.Close();
}
finally
{
this.connected = false;
this.connection = null;
}
}
}
public virtual MReq deserialize <MReq, MRes>(ServiceCallbackHelperParams <MReq, MRes> parms) where MReq : RosMessage where MRes : RosMessage
{
RosMessage msg = RosMessage.Generate(type);
msg.connection_header = new Dictionary <string, string>(parms.connection_header);
MReq t = (MReq)msg;
t.Deserialize(parms.response.Serialized);
return(t);
}
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 static RosService Generate(string rosServiceType)
{
var service = ServiceTypeRegistry.Default.CreateService(rosServiceType);
if (service == null)
{
throw new ArgumentException($"Could not find a RosService class for {rosServiceType}.", nameof(rosServiceType));
}
service.RequestMessage = RosMessage.Generate(service.ServiceType + "__Request");
service.ResponseMessage = RosMessage.Generate(service.ServiceType + "__Response");
return(service);
}
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 async Task <Subscriber> SubscribeAsync(string topic, string messageType, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks = false)
{
if (callbackQueue == null)
{
callbackQueue = ROS.GlobalCallbackQueue;
}
var message = RosMessage.Generate(messageType);
var ops = new SubscribeOptions(topic, message.MessageType, message.MD5Sum(), queueSize, new SubscriptionCallbackHelper <RosMessage>(message.MessageType, cb.SendEvent))
{
callback_queue = callbackQueue,
allow_concurrent_callbacks = allowConcurrentCallbacks
};
ops.callback_queue.AddCallback(cb);
return(await SubscribeAsync(ops));
}
private bool onMessage(Connection conn, byte[] buffer, int size, bool success)
{
if (!success || conn == null || conn != connection)
{
return(false);
}
if (success)
{
RosMessage msg = RosMessage.Generate(parent.msgtype);
msg.Serialized = buffer;
msg.connection_header = getHeader().Values;
handleMessage(msg, true, false);
}
if (success || !connection.transport.getRequiresHeader())
{
connection.read(4, onMessageLength);
}
return(true);
}