/**
* Add a subscriber callback to this connection. There can be many subscribers.
*/
public void AddSubscriber(Type subscriber)
{
//Multiple subscribers for the same topic possible
IsValidSubscriber(subscriber);
string topic = GetMessageTopic(subscriber);
_subscribers.Add(subscriber);
if (_running)
{
//only announce if not announced yet,
if (!m_SubscribedTopics.Contains(topic))
{
//Debug.Log("[ROS WEBSOCKET] Adding Subscriber. Subscribing to " + topic);
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(subscriber), GetMessageType(subscriber)));
m_SubscribedTopics.Add(topic);
}
else
{
Debug.Log("[ROS WEBSOCKET] ALREADY subscribed to " + topic);
}
}
else
{
Debug.Log("[ROS WEBSOCKET] couldn't subscribe to " + topic + ". Websocket not running.");
}
}
public void Publish(string topic, ROSBridgeMsg msg)
{
if (_ws != null)
{
if (m_AnnouncedTopics.Contains(topic))
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
//Debug.Log ("Sending on " + topic);
_ws.Send(s);
}
else
{// if not announced that we're publishing on this topic
//-> go through list of publishers/ subscribers and see if one is not yet advertised, try again
AnnouncePublishersAndSubscribers();
//Try second time otherwise not working
if (m_AnnouncedTopics.Contains(topic))
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
//Debug.Log ("Sending on " + topic);
_ws.Send(s);
}
else
{
Debug.LogWarning("[ROS Websocket Connection] Trying to publish on a topic that is not registered!\n Topic: " + topic);
}
}
}
}
/// <summary>
/// Announces all publishers and subscribers which were previously added to the lists _publishers and _subscriers
/// </summary>
private void AnnouncePublishersAndSubscribers()
{
if (_running && _ws != null)
{
foreach (Type p in _subscribers)
{
string topic = GetMessageTopic(p);
//only announce if not already known that we subscribed
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Subscribing to " + topic);
_ws.Send(ROSBridgeMsg.Subscribe(topic, GetMessageType(p)));
}
m_SubscribedTopics.Add(topic);
}
foreach (Type p in _publishers)
{
string topic = GetMessageTopic(p);
//only announce new publisher if we didn't already announce one for this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Advertising " + topic);
_ws.Send(ROSBridgeMsg.Advertise(topic, GetMessageType(p)));
}
m_AnnouncedTopics.Add(topic);
}
}
else
{
Debug.LogWarning("[ROS WEBSOCKET] COuld not advertise/ subscribe since websocket not running.");
}
}
public void CallService(string service, string args)
{
if (_ws != null)
{
string s = ROSBridgeMsg.CallService(service, args);
//Debug.Log ("Sending " + s);
_ws.Send(s);
}
}
public void Publish(String topic, ROSBridgeMsg msg)
{
if (_ws != null)
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
//Debug.Log ("Sending " + s);
_ws.Send(s);
}
}
private void OnMessage(string s)
{
//Debug.Log ("Got a message " + s);
if ((s != null) && !s.Equals(""))
{
JSONNode node = JSONNode.Parse(s);
//Debug.Log ("Parsed it");
string op = node["op"];
//Debug.Log ("Operation is " + op);
if ("publish".Equals(op))
{
string topic = node["topic"];
//Debug.Log ("Got a message on " + topic);
foreach (Type p in _subscribers)
{
if (topic.Equals(GetMessageTopic(p)))
{
//Debug.Log ("And will parse it " + GetMessageTopic (p));
ROSBridgeMsg msg = ParseMessage(p, node["msg"]);
RenderTask newTask = new RenderTask(p, topic, msg);
lock (_queueLock) {
bool found = false;
for (int i = 0; i < _taskQ.Count; i++)
{
if (_taskQ[i].getTopic().Equals(topic))
{
_taskQ.RemoveAt(i);
_taskQ.Insert(i, newTask);
found = true;
break;
}
}
if (!found)
{
_taskQ.Add(newTask);
}
}
}
}
}
else if ("service_response".Equals(op))
{
Debug.Log("Got service response " + node.ToString());
_serviceName = node["service"];
_serviceValues = (node["values"] == null) ? "" : node["values"].ToString();
}
else
{
Debug.Log("Must write code here for other messages");
}
}
else
{
Debug.Log("Got an empty message from the web socket");
}
}
public void Disconnect()
{
_myThread.Abort();
foreach (Type p in _subscribers)
{
_ws.Send(ROSBridgeMsg.UnSubscribe(GetMessageTopic(p)));
Debug.Log("Sending " + ROSBridgeMsg.UnSubscribe(GetMessageTopic(p)));
}
foreach (Type p in _publishers)
{
_ws.Send(ROSBridgeMsg.UnAdvertise(GetMessageTopic(p)));
Debug.Log("Sending " + ROSBridgeMsg.UnAdvertise(GetMessageTopic(p)));
}
_ws.Close();
}
/**
* Removes a subscriber and disconnects him if the connection is already running.
*/
public void RemoveSubscriber(Type subscriber)
{
if (!_subscribers.Contains(subscriber))
{
return;
}
_subscribers.Remove(subscriber);
string topic = GetMessageTopic(subscriber);
m_SubscribedTopics.Remove(topic);
if (_running)
{
//since multiple subscribers can subscribe to same topic, only send unsubscribe if no more subscribers on this topic
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET]Not subcribing anymore to " + topic);
_ws.Send(ROSBridgeMsg.UnSubscribe(topic));
}
}
}
private void Run()
{
_ws = new WebSocket(_host + ":" + _port);
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
_ws.Connect();
foreach (Type p in _subscribers)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
Debug.Log("Sending " + ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
}
foreach (Type p in _publishers)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
Debug.Log("Sending " + ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
}
while (true)
{
Thread.Sleep(1000);
}
}
/**
* Disconnect from the remote ros environment.
*/
public void Disconnect()
{
Debug.Log("[ROS WEBSOCKET] Disconnecting from ROS");
_myThread.Abort();
foreach (Type p in _subscribers)
{
string topic = GetMessageTopic(p);
//remove this subscriber
if (m_SubscribedTopics.Contains(topic))
{
m_SubscribedTopics.Remove(topic);
}
//if no mo subscribers on this topic
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not subscribing anymore to " + topic);
_ws.Send(ROSBridgeMsg.UnSubscribe(topic));
}
}
foreach (Type p in _publishers)
{
string topic = GetMessageTopic(p);
//remove this advertiser
if (m_AnnouncedTopics.Contains(topic))
{
m_AnnouncedTopics.Remove(topic);
}
//if no more advertiser on this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not publishing on topic: " + topic);
_ws.Send(ROSBridgeMsg.UnAdvertise(topic));
}
}
Debug.Log("[ROS WEBSOCKET] Closing websocket");
_ws.Close();
_running = false;
}
/**
* Removes a publisher from the connection. Disconnects the publisher if connection is already running.
*/
public void RemovePublisher(Type publisher)
{
if (!_publishers.Contains(publisher))
{
return;
}
_publishers.Remove(publisher);
//keep track of all publishers no matter if the topic
string topic = GetMessageTopic(publisher);
m_AnnouncedTopics.Remove(topic);
if (_running)
{
//if we hvae no more publishers on this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not announcing anymore on: " + topic);
_ws.Send(ROSBridgeMsg.UnAdvertise(topic));
}
}
}
/**
* Add a publisher to this connection. There can be many publishers.
*/
public void AddPublisher(Type publisher)
{
IsValidPublisher(publisher);
string topic = GetMessageTopic(publisher);
_publishers.Add(publisher);
if (_running)
{
if (!m_AnnouncedTopics.Contains(topic))
{ // only advertise if needed
Debug.Log("[ROS WEBSOCKET] Adding publisher. Advertising " + topic);
_ws.Send(ROSBridgeMsg.Advertise(topic, GetMessageType(publisher)));
}
//keep track of all publishers (no matter the topic or if the topic already exists )
m_AnnouncedTopics.Add(topic);
}
else
{
Debug.Log("[ROS WEBSOCKET] Could not advertise publisher. Websocket not running.");
}
}
/// <summary>
///
/// </summary>
/// <param name="topic"></param>
/// <param name="msg"></param>
public void Publish(string topic, ROSBridgeMsg msg)
{
Debug.Log("[ROSBridge Message] Publishing some message on topic" + topic);
m_ROS.Publish(topic, msg);
}
public static void CallBack(ROSBridgeMsg msg)
{
}
public RenderTask(Type subscriber, string topic, ROSBridgeMsg msg)
{
_subscriber = subscriber;
_topic = topic;
_msg = msg;
}
private static void Update(Type t, ROSBridgeMsg msg)
{
t.GetMethod("CallBack", BindingFlags.Public | BindingFlags.Static | BindingFlags.FlattenHierarchy).Invoke(null, new object[] { msg });
}
