private void Run()
{
_ws = new WebSocket(_host + ":" + _port);
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
//_ws.CloseAsync();
_ws.Connect();
foreach (Type p in _subscribers)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
if (_debug)
{
Debug.Log("Sending " + ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
}
}
foreach (Type p in _publishers)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
if (_debug)
{
Debug.Log("Sending " + ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
}
}
while (true)
{
Thread.Sleep(10000);
}
}
public virtual void CallBack(ROSBridgeMsg msg)
{
if (OnDataReceived != null)
{
OnDataReceived(msg);
}
}
private void Run()
{
_ws = new WebSocket(_host + ":" + _port);
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
_ws.OnError += _ws_OnError;
_ws.OnOpen += _ws_OnOpen;
_ws.Connect();
while (!connected)
{
Thread.Sleep(10);
}
foreach (var sub in _subscribers)
{
_ws.Send(ROSBridgeMsg.Subscribe(sub.Key.topic, sub.Key.type));
Debug.Log("Sending: " + ROSBridgeMsg.Subscribe(sub.Key.topic, sub.Key.type));
}
foreach (var p in _publishers)
{
_ws.Send(ROSBridgeMsg.AdvertiseTopic(p.topic, p.type));
Debug.Log("Sending " + ROSBridgeMsg.AdvertiseTopic(p.topic, p.type));
}
foreach (var srv in _serviceServers)
{
_ws.Send(ROSBridgeMsg.AdvertiseService(srv.Key.topic, srv.Key.type));
Debug.Log("Sending: " + ROSBridgeMsg.AdvertiseService(srv.Key.topic, srv.Key.type));
}
while (_connected)
{
Thread.Sleep(10);
}
}
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)));
}
foreach (Pair <Type, String> p in _publishersNew)
{
_ws.Send(ROSBridgeMsg.Advertise(p.Second, GetMessageType(p.First)));
Debug.Log("Sending " + ROSBridgeMsg.Publish(p.Second, GetMessageType(p.First)));
}
while (true)
{
Thread.Sleep(1000);
}
}
/// <summary>
/// Disconnect from the remote ros environment.
/// </summary>
public void Disconnect()
{
if (!connected)
{
return;
}
_recvThread.Abort();
foreach (var sub in _subscribers)
{
_ws.Send(ROSBridgeMsg.UnSubscribe(sub.Key.topic));
}
foreach (var p in _publishers)
{
_ws.Send(ROSBridgeMsg.UnAdvertiseTopic(p.topic));
}
foreach (var srv in _serviceServers)
{
_ws.Send(ROSBridgeMsg.UnadvertiseService(srv.Key.topic));
}
_ws.Close();
_msgQueue.Clear();
_connected = false;
}
private void Run()
{
_ws = new WebSocket(_host + ":" + _port);
// _ws.Compression = CompressionMethod.Deflate;
_ws.Log.Level = LogLevel.Trace;
// _ws.Log.File = "socket.log";
_ws.OnError += (sender, e) => { UnityEngine.Debug.LogError("Error: " + e.Message); };
_ws.OnClose += (sender, e) => { UnityEngine.Debug.Log("Connection closed: " + e.Reason); };
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
_ws.Connect();
if (_ws != null)
{
foreach (Type p in _subscribers)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
UnityEngine.Debug.Log("Sending " + ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
}
foreach (Type p in _publishers)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
UnityEngine.Debug.Log("Sending " + ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
}
}
while (_applicationIsPlaying)
{
Thread.Sleep(1000);
}
}
private void Run()
{
#if UNITY_EDITOR
_ws = new WebSocket(_host + ":" + _port);
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
//TODO connecting takes too long
_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)));
}
_connected = true;
while (true)
{
Thread.Sleep(1000);
}
#endif
}
/**
* Add a publisher to this connection. There can be many publishers.
*/
public void AddPublisher(Type publisher)
{
IsValidStaticPublisher(publisher);
string topicName = (string)publisher.GetMethod("GetMessageTopic", BindingFlags.Public | BindingFlags.Static | BindingFlags.FlattenHierarchy).Invoke(null, new object[] { });
List <Type> topic_pubs;
if (static_publishers.TryGetValue(topicName, out topic_pubs))
{
if (!topic_pubs.Contains(publisher))
{
topic_pubs.Add(publisher);
}
}
else
{
topic_pubs = new List <Type>(2);
topic_pubs.Add(publisher);
static_publishers.Add(topicName, topic_pubs);
}
if (_connected)
{
_ws.Send(ROSBridgeMsg.Advertise(topicName, GetMessageType(publisher)));
Debug.Log("Sending " + ROSBridgeMsg.Advertise(topicName, GetMessageType(publisher)));
}
}
/**
* Disconnect from the remote ros environment.
*/
public void Disconnect()
{
_applicationIsPlaying = false;
_myThread.Join();
//_myThread.Abort (); // Abort() does not guarantee that the thread is stopped
if (_ws != null)
{
foreach (Type p in _subscribers)
{
try
{
_ws.Send(ROSBridgeMsg.UnSubscribe(GetMessageTopic(p)));
UnityEngine.Debug.Log("Send " + ROSBridgeMsg.UnSubscribe(GetMessageTopic(p)));
} catch
{
UnityEngine.Debug.LogWarning("Sending " + ROSBridgeMsg.UnSubscribe(GetMessageTopic(p)) + " failed.");
}
}
foreach (Type p in _publishers)
{
try
{
_ws.Send(ROSBridgeMsg.UnAdvertise(GetMessageTopic(p)));
UnityEngine.Debug.Log("Send " + ROSBridgeMsg.UnAdvertise(GetMessageTopic(p)));
} catch
{
UnityEngine.Debug.LogWarning("Sending " + ROSBridgeMsg.UnAdvertise(GetMessageTopic(p)) + " failed.");
}
}
}
_ws.Close();
}
/// <summary>
/// Disconnects from the ROS Bridge and unsubscribes from all subscribed topics.
/// </summary>
public void Disconnect()
{
IsDisconnecting = true;
Debug.Log("Disconnecting from " + _host);
_myThread.Abort();
if (WebSocket == null)
{
return;
}
foreach (ROSBridgeSubscriber subscriber in _subscribers)
{
WebSocket.Send(ROSBridgeMsg.UnSubscribe(subscriber.GetMessageTopic()));
Debug.Log("Sending " + ROSBridgeMsg.UnSubscribe(subscriber.GetMessageTopic()));
}
foreach (ROSBridgePublisher publisher in _publishers)
{
WebSocket.Send(ROSBridgeMsg.UnAdvertise(publisher.GetMessageTopic()));
Debug.Log("Sending " + ROSBridgeMsg.UnAdvertise(publisher.GetMessageTopic()));
}
_subscribers.Clear();
_publishers.Clear();
//fix this in a better way
RobotInterface.Instance.StartCoroutine(Example());
WebSocket.Close();
IsConnected = false;
}
/// <summary>
/// Disconnect from the remote ros environment.
/// </summary>
public void Disconnect()
{
_connected = false;
_myThread.Abort();
foreach (string topicName in static_subscribers.Keys)
{
_ws.Send(ROSBridgeMsg.UnSubscribe(topicName));
Debug.Log("Sending " + ROSBridgeMsg.UnSubscribe(topicName));
}
foreach (string topicName in subscribers.Keys)
{
if (!static_subscribers.ContainsKey(topicName))
{
_ws.Send(ROSBridgeMsg.UnSubscribe(topicName));
Debug.Log("Sending " + ROSBridgeMsg.UnSubscribe(topicName));
}
}
foreach (string topicName in static_publishers.Keys)
{
_ws.Send(ROSBridgeMsg.UnAdvertise(topicName));
Debug.Log("Sending " + ROSBridgeMsg.UnAdvertise(topicName));
}
_ws.Close();
static_subscribers.Clear();
static_publishers.Clear();
subscribers.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 : ROSBridgeLib.ROSMessage, new()
{
MessageCallback CB = (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> sub = new ROSBridgeSubscriber <Tmsg>(topic, messageType);
_subscribers.Add(sub, CB);
_msgQueue.Add(sub.topic, new RenderQueue <MessageTask>(queueSize));
if (connected)
{
_ws.Send(ROSBridgeMsg.Subscribe(sub.topic, sub.type));
}
return(sub);
}
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"];
//if (topic == "/odom") {
// int xxxx = 0;
//}
//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 Publish(String topic, ROSBridgeMsg msg)
{
if (_ws != null)
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
_ws.Send(s);
}
}
/// <summary>
/// Remove a Service server from this connection
/// </summary>
/// <param name="srv"></param>
public void Unadvertise(ROSBridgeServiceProvider srv)
{
if (connected)
{
_ws.Send(ROSBridgeMsg.UnadvertiseService(srv.topic));
}
_serviceServers.Remove(srv);
}
public void RemovePublisher(ROSBridgePublisher publisher)
{
_publishers.Remove(publisher);
if (IsConnected)
{
WebSocket.Send(ROSBridgeMsg.UnAdvertise(publisher.GetMessageTopic()));
}
}
public void RemoveSubcriber(ROSBridgeSubscriber subscriber)
{
_subscribers.Remove(subscriber);
if (IsConnected)
{
WebSocket.Send(ROSBridgeMsg.UnSubscribe(subscriber.GetMessageTopic()));
}
}
public void AddPublisher(ROSBridgePublisher publisher)
{
_publishers.Add(publisher);
if (IsConnected && !IsDisconnecting)
{
WebSocket.Send(ROSBridgeMsg.Advertise(publisher.GetMessageTopic(), publisher.GetMessageType()));
}
}
public void AddSubscriber(ROSBridgeSubscriber subscriber)
{
_subscribers.Add(subscriber);
if (IsConnected && !IsDisconnecting)
{
WebSocket.Send(ROSBridgeMsg.Subscribe(subscriber.GetMessageTopic(), subscriber.GetMessageType()));
}
}
/**
* Add a publisher to this connection. There can be many publishers.
*/
public void AddPublisher(Type publisher)
{
IsValidPublisher(publisher);
_publishers.Add(publisher);
if (_running)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(publisher), GetMessageType(publisher)));
}
}
/**
* Add a subscriber callback to this connection. There can be many subscribers.
*/
public void AddSubscriber(Type subscriber)
{
IsValidSubscriber(subscriber);
_subscribers.Add(subscriber);
if (_running)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(subscriber), GetMessageType(subscriber)));
}
}
public void CallService(string service, string args)
{
if (WebSocket != null && !IsDisconnecting)
{
string s = ROSBridgeMsg.CallService(service, args);
//Debug.Log("Sending " + s);
WebSocket.Send(s);
}
}
/// <summary>
/// Remove a publisher from this connection
/// </summary>
/// <param name="pub"></param>
public void Unadvertise(ROSBridgePublisher pub)
{
if (connected)
{
_ws.Send(ROSBridgeMsg.UnAdvertiseTopic(pub.topic));
}
_publishers.Remove(pub);
}
public void Publish(String topic, ROSBridgeMsg msg)
{
if (WebSocket != null && !IsDisconnecting)
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
// Debug.Log("Sending " + s);
WebSocket.Send(s);
}
}
public void CallService(string service) // added FBF 20/09/2017 include Callservice without parameters
{
if (_ws != null)
{
string s = ROSBridgeMsg.CallService(service);
Debug.Log("Sending " + s);
_ws.Send(s);
}
}
public void CallService(string service, string args)
{
if (_ws != null)
{
string s = ROSBridgeMsg.CallService(service, args);
Debug.Log("Sending " + s);
_ws.Send(s);
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
TendonInitializationMsg tendon = (TendonInitializationMsg)msg;
if (tendon != null)
{
VRUILogic.Instance.AddTendon(tendon.GetTendonID(), tendon.GetWirepoints(),
tendon.GetRoboyParts(), tendon.GetMaxForce());
}
}
/// <summary>
/// Perform a service call with a callback for the response.
/// </summary>
/// <param name="handler">The callback function to invoke when the response of the service call arrives.</param>
/// <param name="service_name">The name of the service to be called.</param>
/// <param name="id">The ID by which this call is distinguished both to the server and by this class.</param>
/// <param name="args">The formatted argument string, as specified by the ROSBridge protocol.</param>
/// <see cref="https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md"/>
public void CallService(JSONServiceResponseHandler handler, string service_name, string id, string args = "[]")
{
if (_ws != null)
{
_pendingServiceResponses[id] = handler;
string s = ROSBridgeMsg.CallService(service_name, id, args);
Debug.Log("Sending " + s);
_ws.Send(s);
}
}
public void Publish(String topic, ROSBridgeMsg msg)
{
// if(_ws != null && _ws.IsConnected && _ws.IsAlive) { //this call take a lot of time
if (_ws != null)
{
string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
// UnityEngine.Debug.Log ("Sending " + s);
_ws.Send(s);
}
}
private void OnMessage(string s)
{
if ((s != null) && !s.Equals(""))
{
JSONNode node = JSONNode.Parse(s);
string op = node["op"];
if ("publish".Equals(op))
{
string topic = node["topic"];
foreach (ROSBridgeSubscriber subscriber in _subscribers)
{
if (topic.Equals(subscriber.GetMessageTopic()))
{
ROSBridgeMsg msg = subscriber.ParseMessage(node["msg"]);
RenderTask newTask = new RenderTask(subscriber, 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");
}
}
