/// <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));
pub.Unadvertise();
}
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));
}
publisher.Unadvertise();
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);
publisher.CreatePublishingThread();
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 <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();
//}
}
}
