public async Task TestChannelsAsync()
{
const string topicName = "/my_test_topic_xyz";
await using var client = await RosClient.CreateAsync(MasterUri, CallerId, CallerUri);
await using var publisher = await RosChannelWriterUtils.CreateWriterAsync <String>(client, topicName);
publisher.LatchingEnabled = true;
var systemState = await client.GetSystemStateAsync();
Assert.True(
systemState.Publishers.Any(tuple => tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
const string msgText = "test message";
publisher.Write(new String(msgText));
await using var subscriber = await RosChannelReaderUtils.CreateReaderAsync <String>(client, topicName);
using var source = new CancellationTokenSource(5000);
await foreach (var msg in subscriber.ReadAllAsync(source.Token))
{
if (msg.Data == msgText)
{
break;
}
}
}
public static async Task WaitForTopicAsync(this RosClient client, string topic,
CancellationToken token = default)
{
if (client == null)
{
throw new ArgumentNullException(nameof(client));
}
if (topic == null)
{
throw new ArgumentNullException(nameof(topic));
}
while (true)
{
try
{
var result = await client.RosMasterClient.GetPublishedTopicsAsync(token : token);
if (result.IsValid && result.Topics.Any(tuple => tuple.name == topic))
{
return;
}
await Task.Delay(200, token);
}
catch (XmlRpcException)
{
}
}
}
public async Task TestGenericChannelReaderAsync()
{
const string topicName = "/my_test_topic_xyz";
await using var client = await RosClient.CreateAsync(MasterUri, CallerId, CallerUri);
await using var publisher = await client.CreateWriterAsync <String>(topicName);
publisher.LatchingEnabled = true;
var systemState = await client.GetSystemStateAsync();
Assert.True(
systemState.Publishers.Any(tuple => tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
const string msgText = "test message";
publisher.Write(new String(msgText));
// reader has no message type, it will use whatever the publisher sends during handshake
await using var subscriber = await client.CreateReaderAsync(topicName);
using var source = new CancellationTokenSource(5000);
await foreach (var msg in subscriber.ReadAllAsync(source.Token))
{
if (msg is String {
Data: msgText
})
public static async Task WaitForServiceAsync(this RosClient client, string service,
CancellationToken token = default)
{
if (client == null)
{
throw new ArgumentNullException(nameof(client));
}
if (service == null)
{
throw new ArgumentNullException(nameof(service));
}
while (true)
{
try
{
var result = await client.RosMasterClient.LookupServiceAsync(service, token);
if (result.IsValid)
{
return;
}
await Task.Delay(200, token);
}
catch (XmlRpcException)
{
}
}
}
public void TestRosClientConnection()
{
using var _ = new RosClient(MasterUri, CallerId, CallerUri);
Uri testMasterUri = new RosNodeClient(CallerId, OtherCallerUri, CallerUri).GetMasterUri().Uri;
Assert.True(testMasterUri == MasterUri);
Assert.Catch <RosUriBindingException>(() => new RosClient(MasterUri, CallerId, CallerUri));
}
public async Task UnadvertiseAsync(RosClient client, CancellationToken token)
{
token.ThrowIfCancellationRequested();
string fullService = service[0] == '/' ? service : $"{client?.CallerId}/{service}";
if (client != null)
{
await client.UnadvertiseServiceAsync(fullService, token);
}
}
// this gets called when we're connected to the master
async void Start()
{
Uri masterUri = new Uri("http://141.3.59.5:11311");
Uri myUri = RosClient.TryGetCallerUriFor(masterUri, 7635) ?? RosClient.TryGetCallerUri(7635);
connectionClient = await RosClient.CreateAsync(masterUri, "/iviz_test", myUri);
scenePublisher = new RosChannelWriter <PlanningScene> {
LatchingEnabled = true
};
await scenePublisher.StartAsync(connectionClient, "/move_group/monitored_planning_scene");
//Logger.LogDebug = Debug.Log;
Logger.Log = Debug.Log;
Logger.LogError = Debug.LogWarning;
Debug.Log($"{this}: Connected!");
while (true)
{
if (this == null)
{
return;
}
// keep checking whether the moveit_test node is on
const string trajectoryService = "/moveit_test/calculate_trajectory";
var systemState = await connectionClient.GetSystemStateAsync();
bool hasService = systemState.Services.Any(service => service.Topic == trajectoryService);
if (hasService)
{
break;
}
Debug.LogWarning($"{this}: Service not detected! Retrying...");
await Task.Delay(2000);
}
Debug.Log($"{this}: Service found. Starting robot and listeners.");
// generate two robots: 'robot' for the real robot, and 'planRobot' for the plan
await GenerateRobotAsync();
// start listening to the joints topic
// these joints apply only for 'robot'.
jointsListener = new RosChannelReader <JointState>();
await jointsListener.StartAsync(connectionClient, "/joint_states");
// here we create a new collision object for the scene
PlanningScene scene = new PlanningScene();
scene.Name = "(noname+)"; // taken from rviz
scene.World.CollisionObjects = new[]
public static void WaitForService(this RosClient client, string service, int timeoutInMs)
{
using CancellationTokenSource tokenSource = new(timeoutInMs);
try
{
client.WaitForService(service, tokenSource.Token);
}
catch (OperationCanceledException)
{
throw new TimeoutException($"Wait for service '{service}' timed out");
}
}
public static void WaitForTopic(this RosClient client, string topic, int timeoutInMs)
{
using CancellationTokenSource tokenSource = new(timeoutInMs);
try
{
client.WaitForTopic(topic, tokenSource.Token);
}
catch (OperationCanceledException)
{
throw new TimeoutException($"Wait for topic '{topic}' timed out");
}
}
public static void WaitForService(this RosClient client, string service, CancellationToken token = default)
{
if (client == null)
{
throw new ArgumentNullException(nameof(client));
}
if (service == null)
{
throw new ArgumentNullException(nameof(service));
}
Task.Run(() => client.WaitForServiceAsync(service, token), token).WaitAndRethrow();
}
public static void WaitForTopic(this RosClient client, string topic, CancellationToken token = default)
{
if (client == null)
{
throw new ArgumentNullException(nameof(client));
}
if (topic == null)
{
throw new ArgumentNullException(nameof(topic));
}
Task.Run(() => client.WaitForTopicAsync(topic, token), token).WaitAndRethrow();
}
public void TestRosClientAdvertisement()
{
const string topicName = "/my_topic";
using (var client = new RosClient(MasterUri, CallerId, CallerUri))
{
client.Advertise(topicName, out RosPublisher <String> _);
var systemState = client.GetSystemState();
Assert.True(systemState.Publishers.Any(tuple =>
tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
}
var newSystemState = new RosMasterClient(MasterUri, CallerId, CallerUri).GetSystemState();
Assert.False(newSystemState.Publishers.Any(tuple =>
tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
}
private async Task ClientStateTask()
{
while (_running)
{
if (EnableTestData)
{
SendTestData();
}
if (RosClient.WebSocket != null && RosClient.WebSocket.State == WebSocketState.Open)
{
await Task.Delay(1000);
continue;
}
// Remove the old topics if they exist
_gpsTopic?.Dispose();
_sonarTopic?.Dispose();
// Create a new connection and topics
try
{
await RosClient.ConnectAsync(new Uri(_configuration["RosBridgeUri"]));
}
catch (Exception e)
{
Console.WriteLine("ROS connection failed:");
Console.WriteLine(e);
await Task.Delay(1000);
continue;
}
_gpsTopic = new RosTopic(RosClient, "/gps", null);
_gpsTopic.RosMessage += GpsTopicOnRosMessage;
_sonarTopic = new RosTopic(RosClient, "/sonar", null);
_sonarTopic.RosMessage += SonarTopicOnRosMessage;
await Task.Delay(1000);
}
}
public async Task TestRosClientAdvertisementAsync()
{
const string topicName = "/my_topic";
await using (var client = await RosClient.CreateAsync(MasterUri, CallerId, CallerUri))
{
await client.AdvertiseAsync <String>(topicName);
var systemState = await client.GetSystemStateAsync();
Assert.True(systemState.Publishers.Any(tuple =>
tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
}
var newSystemState = await new RosMasterClient(MasterUri, CallerId, CallerUri).GetSystemStateAsync();
Assert.False(newSystemState.Publishers.Any(tuple =>
tuple.Topic == topicName && tuple.Members.Contains(CallerId)));
}
public async Task SubscribeAsync(RosClient client, IListener listener, CancellationToken token)
{
if (listener != null)
{
listeners.Add((Listener <T>)listener);
}
token.ThrowIfCancellationRequested();
if (client != null)
{
for (int t = 0; t < NumRetries; t++)
{
try
{
IRosSubscriber subscriber;
(subscriberId, subscriber) = await client.SubscribeAsync <T>(topic, Callback, token : token,
transportHint : TransportHint);
if (bagListener != null)
{
bagId = subscriber.Subscribe(bagListener.EnqueueMessage);
}
Subscriber = subscriber;
break;
}
catch (RoslibException e)
{
Core.Logger.Error($"Failed to subscribe to service (try {t.ToString()}): ", e);
await Task.Delay(WaitBetweenRetriesInMs, token);
}
}
}
else
{
Subscriber = null;
}
}
public async Task AdvertiseAsync(RosClient client, CancellationToken token)
{
token.ThrowIfCancellationRequested();
string fullService = service[0] == '/' ? service : $"{client?.CallerId}/{service}";
if (client != null)
{
for (int t = 0; t < NumRetries; t++)
{
try
{
await client.AdvertiseServiceAsync <T>(fullService, CallbackImpl, token);
break;
}
catch (RoslibException e)
{
Core.Logger.Error($"Failed to advertise service (try {t}): ", e);
await Task.Delay(WaitBetweenRetriesInMs, token);
}
}
}
}
static async Task MainImpl(string[] args)
{
Console.WriteLine("** Starting Iviz.ModelService...");
Uri?masterUri = RosClient.EnvironmentMasterUri;
if (masterUri is null)
{
Console.Error.WriteLine("EE Fatal error: Failed to determine master uri. " +
"Try setting ROS_MASTER_URI to the address of the master.");
return;
}
Uri myUri = RosClient.TryGetCallerUriFor(masterUri) ?? RosClient.TryGetCallerUri();
await using RosClient client = await RosClient.CreateAsync(masterUri, "iviz_model_service", myUri);
bool enableFileSchema;
if (args.Length != 0 && args[0] == "--enable-file-schema")
{
enableFileSchema = true;
Console.Error.WriteLine("WW Uris starting with 'file://' are now accepted. " +
"This makes all your files available to the outside.");
}
else
{
enableFileSchema = false;
}
string?rosPackagePathExtras = null;
string?extrasPath = await GetPathExtras();
if (extrasPath != null && File.Exists(extrasPath))
{
try
{
rosPackagePathExtras = await File.ReadAllTextAsync(extrasPath);
}
catch (IOException e)
{
Console.WriteLine($"EE Extras file '{extrasPath}' could not be read: {e.Message}");
}
}
using var modelServer = new ModelServer(rosPackagePathExtras, enableFileSchema);
if (modelServer.NumPackages == 0)
{
return;
}
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.ModelServiceName,
GetModelResource.RosServiceType);
await client.AdvertiseServiceAsync <GetModelResource>(ModelServer.ModelServiceName,
modelServer.ModelCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.TextureServiceName,
GetModelTexture.RosServiceType);
await client.AdvertiseServiceAsync <GetModelTexture>(ModelServer.TextureServiceName,
modelServer.TextureCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.FileServiceName, GetFile.RosServiceType);
await client.AdvertiseServiceAsync <GetFile>(ModelServer.FileServiceName, modelServer.FileCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.SdfServiceName, GetSdf.RosServiceType);
await client.AdvertiseServiceAsync <GetSdf>(ModelServer.SdfServiceName, modelServer.SdfCallback);
Console.WriteLine("** Done.");
Console.WriteLine("** Iviz.ModelService started with " + modelServer.NumPackages + " ROS package path(s).");
Console.WriteLine("** Standing by for requests.");
await WaitForCancel();
Console.WriteLine();
}
static void GenerateReport([NotNull] StringBuilder builder, [CanBeNull] RosClient client)
{
if (client == null)
{
return;
}
var masterApi = client.RosMasterClient;
builder.Append("<font=Bold>== Master</font> (").Append(masterApi.TotalRequests.ToString("N0"))
.Append(" queries | Ping ")
.Append(masterApi.AvgTimeInQueueInMs).Append(" ms)")
.AppendLine();
long masterReceivedKb = masterApi.BytesReceived / 1000;
long masterSentKb = masterApi.BytesSent / 1000;
builder.Append("<b>Received ").Append(masterReceivedKb.ToString("N0")).Append(" kB | ");
builder.Append("Sent ").Append(masterSentKb.ToString("N0")).Append(" kB</b>").AppendLine();
builder.AppendLine();
var subscriberStats = client.GetSubscriberStatistics();
var publisherStats = client.GetPublisherStatistics();
foreach (var stat in subscriberStats.Topics)
{
builder.Append("<color=#000080ff><font=Bold><< Subscribed to ")
.Append(stat.Topic).Append("</font></color>")
.AppendLine();
builder.Append("<b>Type: </b>[").Append(stat.Type).Append("]").AppendLine();
long totalMessages = 0;
long totalBytes = 0;
long totalDropped = 0;
foreach (var receiver in stat.Receivers)
{
totalMessages += receiver.NumReceived;
totalBytes += receiver.BytesReceived;
totalDropped += receiver.NumDropped;
}
long totalKBytes = totalBytes / 1000;
builder.Append("<b>Received ").Append(totalMessages.ToString("N0")).Append(" msgs | ")
.Append(totalKBytes.ToString("N0")).Append(" kB total</b>").AppendLine();
if (totalDropped != 0)
{
long percentage = totalDropped * 100 / totalMessages;
builder.Append("<b>Dropped ").Append(totalDropped.ToString("N0")).Append(" msgs (")
.Append(percentage).Append("%)</b>").AppendLine();
}
if (stat.Receivers.Count == 0)
{
builder.Append(" (No publishers)").AppendLine().AppendLine();
continue;
}
foreach (var receiver in stat.Receivers)
{
builder.Append(" [");
if (receiver.RemoteUri == client.CallerUri)
{
builder.Append("<i>Me</i> @ ");
}
builder.Append(receiver.RemoteUri.Host).Append(':').Append(receiver.RemoteUri.Port).Append(']');
switch (receiver.Status)
{
case ReceiverStatus.Running:
if (receiver.TransportType != null)
{
builder.Append(receiver.TransportType == TransportType.Tcp
? " TCP"
: " UDP"
);
}
long kbytes = receiver.BytesReceived / 1000;
builder.Append(" | ").Append(kbytes.ToString("N0")).Append("kB");
break;
case ReceiverStatus.ConnectingRpc:
builder.Append(" (Connecting)");
break;
case ReceiverStatus.ConnectingTcp:
builder.Append(" (Connecting to TCP listener)");
break;
case ReceiverStatus.OutOfRetries:
builder.Append(" <color=#ff0000ff>(unreachable)</color>");
break;
default:
builder.Append(" <color=#ff0000ff>(dead)</color>");
break;
}
if (receiver.ErrorDescription != null)
{
var(time, description) = receiver.ErrorDescription;
builder.AppendLine();
builder.Append(" <color=#a52a2aff><b>[").Append(time.ToString("HH:mm:ss"))
.Append("]</b> ").Append(description).Append("</color>");
}
builder.AppendLine();
}
builder.AppendLine();
}
foreach (var stat in publisherStats.Topics)
{
builder.Append("<color=#800000ff><font=Bold>>> Publishing to ").Append(stat.Topic)
.Append("</font></color>")
.AppendLine();
builder.Append("<b>Type: </b>[").Append(stat.Type).Append("]").AppendLine();
long totalMessages = 0;
long totalBytes = 0;
foreach (var sender in stat.Senders)
{
totalMessages += sender.NumSent;
totalBytes += sender.BytesSent;
}
long totalKbytes = totalBytes / 1000;
builder.Append("<b>Sent ").Append(totalMessages.ToString("N0")).Append(" msgs | ")
.Append(totalKbytes.ToString("N0")).Append(" kB</b> total").AppendLine();
if (stat.Senders.Count == 0)
{
builder.Append(" (No subscribers)").AppendLine().AppendLine();
continue;
}
foreach (var sender in stat.Senders)
{
bool isAlive = sender.IsAlive;
builder.Append(" ");
if (sender.RemoteId == client.CallerId)
{
builder.Append("[<i>Me</i> @ ");
}
else
{
builder.Append("[").Append(sender.RemoteId).Append(" @ ");
}
builder.Append(sender.RemoteEndpoint != default
? sender.RemoteEndpoint.Hostname
: "(Unknown address)");
builder.Append(sender.TransportType == TransportType.Tcp
? "] TCP"
: "] UDP"
);
if (isAlive)
{
long kbytes = sender.BytesSent / 1000;
builder.Append(" | ").Append(kbytes.ToString("N0")).Append("kB");
}
else
{
builder.Append(" <color=#ff0000ff>(dead)</color>");
}
builder.AppendLine();
}
builder.AppendLine();
}
}
