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