/// <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 <Tmsg>() where Tmsg : RosMessage
        {
            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 <Tmsg>(eventArgs.Data);
            this.webSocket.OnError   += (sender, eventArgs) => { Debug.LogError("WebSocket Error Message: " + eventArgs.Message); };
            this.webSocket.OnClose   += (sender, eventArgs) => this.OnClose();

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