public void SendTaskAsync_Success()
{
var str = new std_msgs.String();
var header = new
{
callerid = "test",
topic = "mytopic",
md5sum = str.Md5Sum,
type = str.MessageType
};
var stream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(stream, header);
MTcpRosClient.AllInstances.ReceiveAsyncInt32 = (t1, t2) => Observable.Return(stream.ToArray());
MTcpRosClient.AllInstances.SendAsyncByteArray = (t1, t2) => Task.Factory.StartNew(() => t2.Length);
var sock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
var topic = new RosTopicClient <std_msgs.String>("mynode", "mytopic");
topic.StartAsync(sock).Wait();
topic.SendAsync(new std_msgs.String()
{
data = "test"
}).Result.Is(12);
}
public void StartAsync_ReceiveHeaderMismatch()
{
var str = new std_msgs.String();
var header = new
{
callerid = "test",
topic = "mytopic",
md5sum = "aaaaaaaaaaaaa",
type = str.MessageType
};
var stream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(stream, header);
MTcpRosClient.AllInstances.ConnectAsyncStringInt32 = (t1, t2, t3) => Task.Factory.StartNew(() => { });
MTcpRosClient.AllInstances.ReceiveAsyncInt32 = (t1, t2) => Observable.Return(stream.ToArray());
MTcpRosClient.AllInstances.SendAsyncByteArray = (t1, t2) => Task <int> .Factory.StartNew(() => t2.Length);
var server = new RosTopicServer <std_msgs.String>("mynode", "mytopic", new Uri("http://localhost"));
var task = server.StartAsync(new TopicParam()
{
HostName = "test", PortNumber = 1234, ProtocolName = "TCPROS"
});
var ex = AssertEx.Throws <AggregateException>(() => task.Wait());
ex.InnerException.GetType().Is(typeof(RosTopicException));
ex.InnerException.Message.Is("MD5Sum mismatch error");
}
public void StartAsync_SendHeaderError()
{
var str = new std_msgs.String();
var header = new
{
callerid = "test",
topic = "mytopic",
md5sum = str.Md5Sum,
type = str.MessageType
};
var stream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(stream, header);
MTcpRosClient.AllInstances.ReceiveAsyncInt32 = (t1, t2) => Observable.Return(stream.ToArray());
MTcpRosClient.AllInstances.SendAsyncByteArray = (t1, t2) => { throw new InvalidOperationException("Send Error"); };
var sock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
var topic = new RosTopicClient <std_msgs.String>("mynode", "mytopic");
var ex = AssertEx.Throws <AggregateException>(() => topic.StartAsync(sock).Wait());
ex.InnerException.GetType().Is(typeof(InvalidOperationException));
ex.InnerException.Message.Is("Send Error");
}
public void Serialize_EmptyError()
{
var ms = new MemoryStream();
var ex = AssertEx.Throws <RosTopicException>(() => TcpRosHeaderSerializer.Serialize(ms, new object()));
ex.Message.Is("Header does not have properties");
}
public void Deserialize_NotContainsEqual()
{
var msg = new byte[] { 6, 0, 0, 0, 2, 0, 0, 0, 0, 0 };
var ms = new MemoryStream(msg);
var ex = AssertEx.Throws <RosTopicException>(() => TcpRosHeaderSerializer.Deserialize(ms));
ex.Message.Is("Header does not contain '='");
}
public void Deserialize_LengthMismatch2()
{
var msg = new byte[] { 4, 0, 0, 0, 1, 0, 0, 0 };
var ms = new MemoryStream(msg);
var ex = AssertEx.Throws <RosTopicException>(() => TcpRosHeaderSerializer.Deserialize(ms));
ex.Message.Is("Stream length mismatch");
}
public void Deserialize_EmptyError()
{
var msg = new byte[0];
var ms = new MemoryStream(msg);
var ex = AssertEx.Throws <RosTopicException>(() => TcpRosHeaderSerializer.Deserialize(ms));
ex.Message.Is("Stream length is too short");
}
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 StartAsync_Success()
{
var str = new std_msgs.String()
{
data = "test message"
};
var header = new
{
callerid = "test",
topic = "mytopic",
md5sum = str.Md5Sum,
type = str.MessageType
};
var headerStream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(headerStream, header);
var dataStream = new MemoryStream();
var bw = new BinaryWriter(dataStream);
bw.Write(str.SerializeLength);
str.Serialize(bw);
MTcpRosClient.AllInstances.ConnectAsyncStringInt32 = (t1, t2, t3) => Task.Factory.StartNew(() => { });
// 1.Receive Header, 2.Receive Data(std_msgs.String)
int count = 0;
MTcpRosClient.AllInstances.ReceiveAsyncInt32 = (t1, t2) => {
if (count == 0)
{
count++;
return(Observable.Return(headerStream.ToArray()));
}
else
{
return(Observable.Return(dataStream.ToArray()));
}
};
MTcpRosClient.AllInstances.SendAsyncByteArray = (t1, t2) => Task <int> .Factory.StartNew(() => t2.Length);
var server = new RosTopicServer <std_msgs.String>("mynode", "mytopic", new Uri("http://localhost"));
var task = server.StartAsync(new TopicParam()
{
HostName = "test", PortNumber = 1234, ProtocolName = "TCPROS"
});
var subscriber = task.Result;
var rec = subscriber.Timeout(TimeSpan.FromSeconds(3)).First();
rec.data.Is("test message");
}
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 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;
});
}));
}
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());
}
public void SerializeSubscriberHeader_Success()
{
var data = new std_msgs.String()
{
data = "test"
};
var header = new
{
callerid = "test",
topic = "/chatter",
md5sum = data.Md5Sum,
type = data.MessageType
};
var ms = new MemoryStream();
TcpRosHeaderSerializer.Serialize(ms, header);
var array = ms.ToArray();
array.Is(new byte[] {
// length
102, 0, 0, 0,
// caller_id length
13, 0, 0, 0,
// caller_id=test
99, 97, 108, 108, 101, 114, 105, 100, 61, 116, 101, 115, 116,
// topci length
14, 0, 0, 0,
// topic=/chatter
116, 111, 112, 105, 99, 61, 47, 99, 104, 97, 116, 116, 101, 114,
// md5sum length
39, 0, 0, 0,
// md5sum=992ce8a1687cec8c8bd883ec73ca41d1
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,
// type length
20, 0, 0, 0,
// type=std_msgs/String
116, 121, 112, 101, 61, 115, 116, 100, 95, 109, 115, 103, 115, 47, 83, 116, 114, 105, 110, 103
});
ms.Close();
}
public void StartAsync_ReceiveHeaderNotEnoughtMemberError()
{
var str = new std_msgs.String();
var header = new
{
callerid = "test",
type = str.MessageType
};
var stream = new MemoryStream();
TcpRosHeaderSerializer.Serialize(stream, header);
MTcpRosClient.AllInstances.ReceiveAsyncInt32 = (t1, t2) => Observable.Return(stream.ToArray());
var sock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
var topic = new RosTopicClient <std_msgs.String>("mynode", "mytopic");
var ex = AssertEx.Throws <AggregateException>(() => topic.StartAsync(sock).Wait());
ex.InnerException.GetType().Is(typeof(RuntimeBinderException));
}
public void DesrializeSubscriberHeader_Success()
{
var msg = new byte[] {
// length
102, 0, 0, 0,
// caller_id length
13, 0, 0, 0,
// caller_id=test
99, 97, 108, 108, 101, 114, 105, 100, 61, 116, 101, 115, 116,
// topci length
14, 0, 0, 0,
// topic=/chatter
116, 111, 112, 105, 99, 61, 47, 99, 104, 97, 116, 116, 101, 114,
// md5sum length
39, 0, 0, 0,
// md5sum=992ce8a1687cec8c8bd883ec73ca41d1
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,
// type length
20, 0, 0, 0,
// type=std_msgs/String
116, 121, 112, 101, 61, 115, 116, 100, 95, 109, 115, 103, 115, 47, 83, 116, 114, 105, 110, 103
};
var ms = new MemoryStream(msg);
dynamic header = TcpRosHeaderSerializer.Deserialize(ms);
var data = new std_msgs.String()
{
data = "test"
};
AssertEx.Is(header.callerid, "test");
AssertEx.Is(header.topic, "/chatter");
AssertEx.Is(header.md5sum, data.Md5Sum);
AssertEx.Is(header.type, data.MessageType);
ms.Close();
}
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);
}