private Task <IObservable <TMessage> > ConnectToPublisherAsync(bool nodelay)
{
var last = _client.ReceiveAsync()
.Take(1)
.Select(x => TcpRosHeaderSerializer.Deserialize(new MemoryStream(x)))
.PublishLast();
last.Connect();
var dummy = new TMessage();
var sendHeader = new
{
callerid = NodeId,
topic = TopicName,
md5sum = dummy.Md5Sum,
type = dummy.MessageType,
tcp_nodelay = nodelay ? "1" : "0"
};
var stream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(stream, sendHeader);
var tcs = new TaskCompletionSource <IObservable <TMessage> >();
_client.SendAsync(stream.ToArray())
.ContinueWith(task =>
{
_logger.Debug("ConnectToPublisherAsync Sent");
if (task.Status == TaskStatus.RanToCompletion)
{
try
{
var recvHeader = last.Timeout(TimeSpan.FromMilliseconds(Ros.TopicTimeout)).First();
tcs.SetResult(CreateSubscriber(recvHeader));
}
catch (TimeoutException ex)
{
_logger.Error("Receive Header Timeout Error", ex);
tcs.SetException(ex);
}
catch (RosTopicException ex)
{
_logger.Error("Header Deserialize Error", ex);
tcs.SetException(ex);
}
}
else if (task.Status == TaskStatus.Faulted)
{
tcs.SetException(task.Exception.InnerException);
}
});
return(tcs.Task);
}
private Task <IObservable <Unit> > ConnectToService()
{
dynamic header = _client.ReceiveAsync()
.Take(1)
.Select(b => TcpRosHeaderSerializer.Deserialize(new MemoryStream(b)))
.Timeout(TimeSpan.FromMilliseconds(Ros.TopicTimeout))
.First();
var dummy = new TService();
_logger.Info(m => m("Receive Header {0}", header.ToString()));
if (header.service != _serviceName)
{
_logger.Error(m => m("ServiceName mismatch error, expected={0} actual={1}", _serviceName, header.topic));
throw new RosTopicException("ServiceName mismatch error");
}
if (header.md5sum != "*" && header.md5sum != dummy.Md5Sum)
{
_logger.Error(m => m("MD5Sum mismatch error, expected={0} actual={1}", dummy.Md5Sum, header.md5sum));
throw new RosTopicException("MD5Sum mismatch error");
}
var sendHeader = new
{
callerid = NodeId,
md5sum = dummy.Md5Sum,
service = _serviceName,
type = dummy.ServiceType
};
var ms = new MemoryStream();
TcpRosHeaderSerializer.Serialize(ms, sendHeader);
return(_client.SendAsync(ms.ToArray())
.ContinueWith(task =>
{
_logger.Info("SendTaskAsync ContinueWith");
return _client.ReceiveAsync()
.Select(b =>
{
var res = Invoke(new MemoryStream(b));
var array = res.ToArray();
_client.SendAsync(array).Wait();
return Unit.Default;
});
}));
}
public IMessage Invoke(IMessage request)
{
var response = _client.ReceiveAsync(offset: 1)
.Select(x =>
{
var res = Service.CreateResponse();
var br = new BinaryReader(new MemoryStream(x));
br.ReadInt32();
res.Deserialize(br);
return(res);
})
.Take(1)
.PublishLast();
response.Connect();
var ms = new MemoryStream();
var bw = new BinaryWriter(ms);
bw.Write(request.SerializeLength);
request.Serialize(bw);
var senddata = ms.ToArray();
_client.SendAsync(senddata).Wait(TimeSpan.FromMilliseconds(Ros.TopicTimeout));
return(response.Timeout(TimeSpan.FromMilliseconds(Ros.TopicTimeout)).First());
}
public Task <IObservable <byte[]> > StartAsync(Socket socket, bool latching = false)
{
_client = new TcpRosClient(socket);
return(_client.ReceiveAsync()
.Take(1)
.Timeout(TimeSpan.FromMilliseconds(Ros.TopicTimeout))
.Select(x => OnReceivedHeader(x, latching))
.ToTask());
}
public void ReceiveAsObservable_Success()
{
var data = new byte[] { 179, 0, 0, 0, 40, 0, 0, 0, 99, 97, 108, 108, 101, 114, 105, 100,
61, 47, 114, 111, 115, 106, 97, 118, 97, 95, 116, 117, 116, 111, 114, 105, 97, 108,
95, 112, 117, 98, 115, 117, 98, 47, 116, 97, 108, 107, 101, 114, 14, 0, 0, 0, 116,
111, 112, 105, 99, 61, 47, 99, 104, 97, 116, 116, 101, 114, 39, 0, 0, 0, 109, 100,
53, 115, 117, 109, 61, 57, 57, 50, 99, 101, 56, 97, 49, 54, 56, 55, 99, 101, 99,
56, 99, 56, 98, 100, 56, 56, 51, 101, 99, 55, 51, 99, 97, 52, 49, 100, 49, 20, 0,
0, 0, 116, 121, 112, 101, 61, 115, 116, 100, 95, 109, 115, 103, 115, 47, 83, 116,
114, 105, 110, 103, 32, 0, 0, 0, 109, 101, 115, 115, 97, 103, 101, 95, 100, 101, 102,
105, 110, 105, 116, 105, 111, 110, 61, 115, 116, 114, 105, 110, 103, 32, 100, 97, 116,
97, 10, 10, 10, 0, 0, 0, 108, 97, 116, 99, 104, 105, 110, 103, 61, 48 };
var scheduler = new TestScheduler();
MAsyncSocketExtensions.ReceiveAsObservableSocketIScheduler =
(t1, t2) => scheduler.CreateHotObservable(OnNext(10, data));
var observer = scheduler.CreateObserver <TcpRosHeader>();
var client = new TcpRosClient();
var result = client.ReceiveAsync()
.Select(x => TcpRosHeaderSerializer.Deserialize(new MemoryStream(x)))
.Subscribe(observer);
scheduler.AdvanceTo(10);
var header = new
{
callerid = "/rosjava_tutorial_pubsub/talker",
latching = "0",
md5sum = "992ce8a1687cec8c8bd883ec73ca41d1",
message_definition = "string data\n\n",
topic = "/chatter",
type = "std_msgs/String",
};
observer.Messages.Count.Is(1);
dynamic actual = observer.Messages.First().Value.Value;
actual.callerid.Is(header.callerid);
actual.latching.Is(header.latching);
actual.md5sum.Is(header.md5sum);
actual.message_definition.Is(header.message_definition);
actual.topic.Is(header.topic);
actual.type.Is(header.type);
}
private IObservable <byte[]> OnReceivedHeader(byte[] data, bool latching)
{
_logger.Debug("OnReceivedHeader");
var dummy = new TMessage();
dynamic reqHeader = TcpRosHeaderSerializer.Deserialize(new MemoryStream(data));
if (reqHeader.topic != TopicName)
{
_logger.Error(m => m("TopicName mismatch error, expected={0} actual={1}", TopicName, reqHeader.topic));
throw new RosTopicException("TopicName mismatch error");
}
if (reqHeader.type != dummy.MessageType)
{
_logger.Error(m => m("TopicType mismatch error, expected={0} actual={1}", dummy.MessageType, reqHeader.type));
throw new RosTopicException("TopicType mismatch error");
}
if (reqHeader.md5sum != dummy.Md5Sum)
{
_logger.Error(m => m("MD5Sum mismatch error, expected={0} actual={1}", dummy.Md5Sum, reqHeader.md5sum));
throw new RosTopicException("MD5Sum mismatch error");
}
if (reqHeader.HasMember("tcp_nodelay"))
{
_client.SetNodelayOption(reqHeader.tcp_nodelay == "1");
}
var resHeader = new
{
callerid = NodeId,
latching = latching ? "1":"0",
md5sum = dummy.Md5Sum,
message_definition = dummy.MessageDefinition,
topic = TopicName,
type = dummy.MessageType
};
var ms = new MemoryStream();
TcpRosHeaderSerializer.Serialize(ms, resHeader);
_client.SendAsync(ms.ToArray()).Wait();
Connected = true;
return(_client.ReceiveAsync());
}
private Task <ServiceProxy <TService> > ConnectToServiceServer <TService>(string serviceName, TcpRosClient tcpClient)
where TService : IService, new()
{
var tcs = new TaskCompletionSource <ServiceProxy <TService> >();
var receiveHeaderObs = tcpClient.ReceiveAsync()
.Select(x => TcpRosHeaderSerializer.Deserialize(new MemoryStream(x)))
.Take(1)
.PublishLast();
receiveHeaderObs.Connect();
var service = new TService();
var sendHeader = new
{
callerid = NodeId,
md5sum = service.Md5Sum,
service = serviceName
};
var sendHeaderStream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(sendHeaderStream, sendHeader);
tcpClient.SendAsync(sendHeaderStream.ToArray())
.ContinueWith(sendTask =>
{
if (sendTask.Status == TaskStatus.RanToCompletion)
{
try
{
var dummy = new TService();
dynamic recvHeader = receiveHeaderObs.Timeout(TimeSpan.FromMilliseconds(Ros.TopicTimeout)).First();
if (recvHeader.HasMember("service") && recvHeader.service != serviceName)
{
_logger.Error(m => m("ServiceName mismatch error, expected={0} actual={1}", serviceName, recvHeader.topic));
throw new RosTopicException("ServiceName mismatch error");
}
if (recvHeader.md5sum != "*" && recvHeader.md5sum != dummy.Md5Sum)
{
_logger.Error(m => m("MD5Sum mismatch error, expected={0} actual={1}", dummy.Md5Sum, recvHeader.md5sum));
throw new RosTopicException("MD5Sum mismatch error");
}
var proxy = new ServiceProxy <TService>(NodeId, serviceName, service, tcpClient);
tcs.SetResult(proxy);
}
catch (Exception ex)
{
tcs.SetException(ex);
}
}
else if (sendTask.Status == TaskStatus.Faulted)
{
tcs.SetException(sendTask.Exception.InnerException);
}
});
return(tcs.Task);
}