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 Task <int> SendAsync(TMessage data)
{
if (!Connected)
{
_logger.Error(m => m("Not Connected"));
throw new InvalidOperationException("Not Connected");
}
var ms = new MemoryStream();
var bw = new BinaryWriter(ms);
bw.Write(data.SerializeLength);
data.Serialize(bw);
return(_client.SendAsync(ms.ToArray()));
}
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());
}
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);
}
public void Receive_Error()
{
var listener = new TcpRosListener(0);
listener.AcceptAsync()
.Do(_ => Console.WriteLine("Connected"))
.Subscribe(s =>
{
new TcpRosClient(s).ReceiveAsync()
.Subscribe(data => Console.WriteLine("Received:{0}", data),
ex => Console.WriteLine("Received Error: {0}", ex.Message));
Thread.Sleep(TimeSpan.FromSeconds(3));
s.Close(); //3秒待って閉じる。
},
ex => Console.WriteLine("Receive Error = {0}", ex.Message));
int port = listener.EndPoint.Port;
var client3 = new TcpRosClient();
client3.ConnectAsync("localhost", port).Wait();
var sendData = new byte[] { 1, 0, 0, 0, 1 };
client3.SendAsync(sendData).Wait();
Thread.Sleep(TimeSpan.FromSeconds(5));
try
{
client3.SendAsync(sendData).Wait();
}
catch (Exception ex)
{
Console.WriteLine("Send Error:{0}", ex.Message);
}
}
public void SendAndReceive()
{
var listener = new TcpRosListener(0);
listener.AcceptAsync()
.Do(_ => Console.WriteLine("Connected"))
.Subscribe(s => new TcpRosClient(s).ReceiveAsync()
.Subscribe(data => Console.WriteLine("Received:{0}", data)));
int port = listener.EndPoint.Port;
Console.WriteLine("Press Any Key 3");
Console.ReadKey();
var client3 = new TcpRosClient();
client3.ConnectAsync("localhost", port).Wait();
var sendData = new byte[] { 1, 0, 0, 0, 1 };
client3.SendAsync(sendData).Wait();
}
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);
}