/// <summary> /// Add a subscriber callback to this connection. There can be many subscribers. /// </summary> /// <typeparam name="Tmsg">Message type used in the callback</typeparam> /// <param name="sub">Subscriber</param> /// <param name="callback">Method to call when a message matching the given subscriber is received</param> public ROSBridgeSubscriber <Tmsg> Subscribe <Tmsg>(string topic, ROSMessageCallback <Tmsg> callback, uint queueSize = 0) where Tmsg : ROSMessage, new() { MessageCallback MessageCallback = (ROSMessage msg) => { Tmsg message = msg as Tmsg; callback(message); }; var getMessageType = typeof(Tmsg).GetMethod("GetMessageType"); if (getMessageType == null) { Debug.LogError("Could not retrieve method GetMessageType() from " + typeof(Tmsg).ToString()); return(null); } string messageType = (string)getMessageType.Invoke(null, null); if (messageType == null) { Debug.LogError("Could not retrieve valid message type from " + typeof(Tmsg).ToString()); return(null); } ROSBridgeSubscriber <Tmsg> subscriber = new ROSBridgeSubscriber <Tmsg>(topic, messageType); this.subscribers.Add(subscriber, MessageCallback); this.msgQueue.Add(subscriber.Topic, new RenderQueue <MessageTask>(queueSize)); if (this.IsConnected) { this.webSocket.Send(ROSBridgeMsg.Subscribe(subscriber.Topic, subscriber.Type)); } return(subscriber); }
/// <summary> /// Connect to the remote ros environment. /// </summary> public void Connect() { if (this.IsConnected) { return; } string url = "ws://" + this.host + ":" + this.port; if (!CanConnect(url)) { throw new Exception("Cannot connect. url=" + url); } this.webSocket = new WebSocket(url); this.webSocket.OnOpen += (sender, eventArgs) => { Debug.Log("WebSocket Open url=" + url); }; this.webSocket.OnMessage += (sender, eventArgs) => this.OnMessage(eventArgs.Data); this.webSocket.OnError += (sender, eventArgs) => { Debug.Log("WebSocket Error Message: " + eventArgs.Message); }; this.webSocket.OnClose += (sender, eventArgs) => { Debug.Log("WebSocket Close"); }; // this.webSocket.Connect(); this.webSocket.ConnectAsync(); DateTime startTime = DateTime.Now; while (this.webSocket.ReadyState != WebSocketState.Open) { if ((DateTime.Now - startTime).TotalMilliseconds > ConnectionTimeOut) { AddCannotConnectUrlList(url); SIGVerseLogger.Error("Failed to connect. IP=" + this.host + ", Port=" + this.port + " (Time out)"); throw new Exception("Failed to connect. IP=" + this.host + ", Port=" + this.port + " (Time out)"); } Thread.Sleep(100); } if (!this.webSocket.IsAlive) { Debug.Log("Error: Connection was faild."); } else { Debug.Log("Connected to ROSbridge server"); foreach (var sub in this.subscribers) { this.webSocket.Send(ROSBridgeMsg.Subscribe(sub.Key.Topic, sub.Key.Type)); Debug.Log("Sending: " + ROSBridgeMsg.Subscribe(sub.Key.Topic, sub.Key.Type)); } foreach (ROSBridgePublisher pub in this.publishers) { this.webSocket.Send(ROSBridgeMsg.AdvertiseTopic(pub.Topic, pub.Type)); Debug.Log("Sending " + ROSBridgeMsg.AdvertiseTopic(pub.Topic, pub.Type)); } foreach (var srv in this.serviceProviders) { this.webSocket.Send(ROSBridgeMsg.AdvertiseService(srv.Key.Name, srv.Key.Type)); Debug.Log("Sending: " + ROSBridgeMsg.AdvertiseService(srv.Key.Name, srv.Key.Type)); } this.isConnected = true; foreach (ROSBridgePublisher pub in this.publishers) { pub.CreatePublishingThread(); } } }