/// <summary>
        /// Disconnect from the remote ros environment.
        /// </summary>
        public void Disconnect()
        {
            if (!this.IsConnected)
            {
                return;
            }

            //	this.sendMsgThread.Abort();

            this.isConnected = false;
            Thread.Sleep(15);

            foreach (var sub in this.subscribers)
            {
                this.webSocket.Send(ROSBridgeMsg.UnSubscribe(sub.Key.Topic));
            }
            foreach (var pub in this.publishers)
            {
                this.webSocket.Send(ROSBridgeMsg.UnAdvertiseTopic(pub.Topic));
            }
            foreach (var srv in this.serviceProviders)
            {
                this.webSocket.Send(ROSBridgeMsg.UnadvertiseService(srv.Key.Name));
            }

            this.webSocket.Close();
            this.msgQueue.Clear();
        }
        /// <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>
        /// Remove a Service server from this connection
        /// </summary>
        /// <param name="serviceProvider"></param>
        public void Unadvertise(ROSBridgeServiceProvider serviceProvider)
        {
            if (this.IsConnected)
            {
                this.webSocket.Send(ROSBridgeMsg.UnadvertiseService(serviceProvider.Name));
            }

            this.serviceProviders.Remove(serviceProvider);
        }
        /// <summary>
        /// Remove a publisher from this connection
        /// </summary>
        /// <param name="publisher"></param>
        public void Unadvertise(ROSBridgePublisher publisher)
        {
            if (this.IsConnected)
            {
                this.webSocket.Send(ROSBridgeMsg.UnAdvertiseTopic(publisher.Topic));
            }

            this.publishers.Remove(publisher);
        }
        /// <summary>
        /// Remove a subscriber callback from this connection.
        /// </summary>
        /// <param name="subscriber"></param>
        public void Unsubscribe(ROSBridgeSubscriber subscriber)
        {
            if (subscriber == null)
            {
                return;
            }

            this.subscribers.Remove(subscriber);
            this.msgQueue.Remove(subscriber.Topic);

            if (this.IsConnected)
            {
                this.webSocket.Send(ROSBridgeMsg.UnSubscribe(subscriber.Topic));
            }
        }
        /// <summary>
        /// Add a publisher to this connection. There can be many publishers.
        /// </summary>
        /// <typeparam name="Tpub">Publisher type to advertise</typeparam>
        /// <param name="topic">Topic to advertise on</param>
        /// <returns>A publisher which can be used to broadcast data on the given topic</returns>
        public ROSBridgePublisher <Tmsg> Advertise <Tmsg>(string topic, uint queueSize = 0) where Tmsg : ROSMessage
        {
            ROSBridgePublisher <Tmsg> publisher = (ROSBridgePublisher <Tmsg>)Activator.CreateInstance(typeof(ROSBridgePublisher <Tmsg>), new object[] { topic, queueSize });

            publisher.SetConnection(this);

            this.publishers.Add(publisher);

            if (this.IsConnected)
            {
                this.webSocket.Send(ROSBridgeMsg.AdvertiseTopic(publisher.Topic, publisher.Type));
            }

            return(publisher);
        }
        /// <summary>
        /// Should be called at least once each frame. Calls any available callbacks for received messages.
        /// Note: MUST be called from Unity's main thread!
        /// </summary>
        public void Render()
        {
            float startTime   = Time.realtimeSinceStartup;              // time at start of this frame
            float maxDuration = 0.5f * Time.fixedDeltaTime;             // max time we want to spend working
            float elapsedTime = 0.0f;                                   // time spent so far processing messages

            while (elapsedTime < maxDuration)
            {
                // get queued work to do
                List <MessageTask> msgTasks = this.MessagePump();
                List <ServiceTask> svcTasks = this.ServicePump();

                // bail if we have no work to do
                if (msgTasks.Count == 0 && svcTasks.Count == 0)
                {
                    break;
                }

                // call all msg subsriber callbacks
                foreach (var msgTask in msgTasks)
                {
                    this.subscribers[msgTask.getSubscriber()](msgTask.getMsg());
                }

                // call all svc handlers
                foreach (var svcTask in svcTasks)
                {
                    ServiceResponse response = null;

                    // invoke service handler
                    bool success = this.serviceProviders[svcTask.Service](svcTask.ServiceArgs, out response);

                    Debug.Log("Sending service response: \n" + ROSBridgeMsg.ServiceResponse(success, svcTask.Service.Name, svcTask.Id, JsonUtility.ToJson(response)));
                    // send response
//					this.webSocket.SendAsync(ROSBridgeMsg.ServiceResponse(success, svcTask.Service.Name, svcTask.Id, JsonUtility.ToJson(response)), null);
                    this.webSocket.Send(ROSBridgeMsg.ServiceResponse(success, svcTask.Service.Name, svcTask.Id, JsonUtility.ToJson(response)));
                }

                elapsedTime = Time.realtimeSinceStartup - startTime;
            }
        }
        /// <summary>
        /// Add a Service server to this connection. There can be many servers, but each service should only have one.
        /// </summary>
        /// <typeparam name="Tsrv">ServiceProvider type</typeparam>
        /// <typeparam name="Targ">Message type containing parameters for this service</typeparam>
        /// <typeparam name="Tres">Message type containing response data returned by this service</typeparam>
        /// <param name="srv">The service to advertise</param>
        /// <param name="callback">Method to invoke when the service is called</param>
        public ROSBridgeServiceProvider <Targ> Advertise <Tsrv, Targ, Tres>(string service, ROSServiceCallback <Targ, Tres> callback) where Tsrv : ROSBridgeServiceProvider <Targ> where Targ : ServiceArgs where Tres : ServiceResponse, new()
        {
            ServiceCallback ServiceCallback = (ServiceArgs args, out ServiceResponse response) =>
            {
                Targ request = (Targ)args;
                Tres res     = new Tres();
                bool success = callback(request, out res);
                response = res;
                return(success);
            };

            Tsrv srv = (Tsrv)Activator.CreateInstance(typeof(Tsrv), new object[] { service });

            this.serviceProviders.Add(srv, ServiceCallback);
            this.svcQueue.Add(srv.Name, new RenderQueue <ServiceTask>(0));

            if (this.IsConnected)
            {
                this.webSocket.Send(ROSBridgeMsg.AdvertiseService(srv.Name, srv.Type));
            }

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