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(); } }