public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage message, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time)
{
if (was_full)
{
was_full = false;
}
var i = new Item
{
helper = helper,
message = message,
nonconst_need_copy = nonconst_need_copy,
receipt_time = receipt_time
};
lock ( queue )
{
if (FullNoLock())
{
queue.Dequeue();
was_full = true;
}
queue.Enqueue(i);
}
}
internal MessageAndSerializerFunc(RosMessage msg, TopicManager.SerializeFunc serfunc, bool ser, bool nc)
{
this.msg = msg;
this.serfunc = serfunc;
serialize = ser;
nocopy = nc;
}
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 void SendEvent(RosMessage msg)
{
if (Event != null)
{
Event(msg);
}
else
{
logger.LogError($"{nameof(Event)} is null");
}
}
public void SendEvent(RosMessage msg)
{
if (Event != null)
{
Event(msg);
}
else
{
ROS.Error()($"[{ThisNode.Name}] {nameof( Event )} is null");
}
}
public bool call(MReq req, ref MRes resp)
{
RosMessage iresp = resp;
bool r = call(req, ref iresp);
if (iresp != null)
{
resp = (MRes)iresp;
}
return(r);
}
public void publish(Publication p, RosMessage msg, SerializeFunc serfunc = null)
{
if (msg == null)
{
return;
}
if (serfunc == null)
{
serfunc = msg.Serialize;
}
if (p.connection_header == null)
{
p.connection_header = new Header {
Values = new Dictionary <string, string>()
};
p.connection_header.Values["type"] = p.DataType;
p.connection_header.Values["md5sum"] = p.Md5sum;
p.connection_header.Values["message_definition"] = p.MessageDefinition;
p.connection_header.Values["callerid"] = ThisNode.Name;
p.connection_header.Values["latching"] = Convert.ToString(p.Latch);
}
if (!ROS.ok || shuttingDown)
{
return;
}
if (p.HasSubscribers || p.Latch)
{
bool nocopy = false;
bool serialize = false;
if (msg != null && msg.MessageType != "undefined/unknown")
{
p.getPublishTypes(ref serialize, ref nocopy, msg.MessageType);
}
else
{
serialize = true;
}
p.publish(new MessageAndSerializerFunc(msg, serfunc, serialize, nocopy));
if (serialize)
{
PollManager.Instance.poll_set.ContinueThreads();
}
}
else
{
p.incrementSequence();
}
}
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 void Publish(Publication p, RosMessage msg, SerializeFunc serfunc = null)
{
if (msg == null)
{
return;
}
if (serfunc == null)
{
serfunc = msg.Serialize;
}
if (p.connectionHeader == null)
{
var fields = new Dictionary <string, string>()
{
["type"] = p.DataType,
["md5sum"] = p.Md5Sum,
["message_definition"] = p.MessageDefinition,
["callerid"] = ThisNode.Name,
["latching"] = p.Latch ? "1" : "0"
};
p.connectionHeader = new Header(fields);
}
if (!ROS.OK || shuttingDown)
{
return;
}
if (p.HasSubscribers || p.Latch)
{
bool nocopy = false;
bool serialize = false;
if (msg != null && msg.MessageType != "undefined/unknown")
{
p.GetPublishTypes(ref serialize, ref nocopy, msg.MessageType);
}
else
{
serialize = true;
}
p.Publish(new MessageAndSerializerFunc(msg, serfunc, serialize, nocopy));
}
else
{
p.IncrementSequence();
}
}
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 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));
}
public virtual async Task ProcessResponse(RosMessage msg, bool success)
{
byte[] buf;
if (success)
{
msg.Serialized = msg.Serialize();
buf = new byte[msg.Serialized.Length + 1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
msg.Serialized.CopyTo(buf, 5);
Array.Copy(BitConverter.GetBytes(msg.Serialized.Length), 0, buf, 1, 4);
}
else
{
buf = new byte[1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
Array.Copy(BitConverter.GetBytes(0), 0, buf, 1, 4);
}
await connection.WriteBlock(buf, 0, buf.Length, cancel);
}
public virtual void processResponse(RosMessage msg, bool success)
{
byte[] buf;
if (success)
{
msg.Serialized = msg.Serialize();
buf = new byte[msg.Serialized.Length + 1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
msg.Serialized.CopyTo(buf, 5);
Array.Copy(BitConverter.GetBytes(msg.Serialized.Length), 0, buf, 1, 4);
}
else
{
buf = new byte[1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
Array.Copy(BitConverter.GetBytes(0), 0, buf, 1, 4);
}
connection.write(buf, buf.Length, onResponseWritten);
}
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);
}
protected void InitSubtypes(RosMessage request, RosMessage response)
{
RequestMessage = request;
ResponseMessage = response;
}
protected RosMessage GeneralInvoke(RosServiceDelegate invocation, RosMessage m)
{
return(invocation.Invoke(m));
}
public abstract void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time);
public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool wasFull, TimeData receiptTime)
{
throw new NotImplementedException();
}
public virtual void call(RosMessage msg)
{
Callback.SendEvent(msg);
}
public override bool Equals(RosMessage msg)
{
return(Equals(msg as FeedbackActionMessage <TFeedback>));
}
public override bool Equals(RosMessage msg)
{
return(Equals(msg as GoalActionMessage <TGoal>));
}
internal long HandleMessage(
RosMessage msg,
bool ser,
bool nocopy,
IDictionary <string, string> connectionHeader,
PublisherLink link
)
{
RosMessage t = null;
long drops = 0;
TimeData receipt_time = ROS.GetTime().data;
if (msg.Serialized != null) // will be null if self-subscribed
{
msg.Deserialize(msg.Serialized);
}
lock (gate)
{
foreach (CallbackInfo info in callbacks)
{
string ti = info.Helper.type;
if (nocopy || ser)
{
t = msg;
t.connection_header = msg.connection_header;
t.Serialized = null;
bool wasFull = false;
bool nonconst_need_copy = callbacks.Count > 1;
info.SubscriptionQueue.AddToCallbackQueue(info.Helper, t, nonconst_need_copy, ref wasFull, receipt_time);
if (wasFull)
{
++drops;
}
else
{
info.CallbackQueue.AddCallback(info.SubscriptionQueue, info.SubscriptionQueue);
}
}
}
}
if (t != null && link.Latched)
{
LatchInfo li = new LatchInfo
{
Message = t,
Link = link,
ConnectionHeader = connectionHeader,
ReceiptTime = receipt_time
};
if (latchedMessages.ContainsKey(link))
{
latchedMessages[link] = li;
}
else
{
latchedMessages.Add(link, li);
}
}
return(drops);
}
public override bool Equals(RosMessage msg)
{
return(Equals(msg as WrappedFeedbackMessage <T>));
}
