diff --git a/src/client/ws.py b/src/client/connection.py similarity index 68% rename from src/client/ws.py rename to src/client/connection.py index 5c1c870..1f1f8fe 100644 --- a/src/client/ws.py +++ b/src/client/connection.py @@ -2,31 +2,25 @@ import threading import string import sys +import json +import copy from twisted.internet import reactor from autobahn.twisted.websocket import WebSocketClientProtocol from autobahn.twisted.websocket import WebSocketClientFactory -test_msg = """ -{"to": "$to", - "from": "$fr", - "topic": "/foo/state", - "type": "geometry_msgs/Point", - "msg": "{'x': 0, 'y': 0, 'z': 0}", - "stamp": $t} -""" - - class MMClient(WebSocketClientProtocol): client = None + updates = dict() def onConnect(self, reponse): MMClient.client = self def onMessage(self, payload, is_binary): if not is_binary: - print payload + data = json.loads(payload) + MMClient.updates[data["topic"]] = payload @staticmethod def send_message(payload): @@ -42,6 +36,7 @@ def __init__(self, host, port, name): self.name = name self.url = "ws://{}:{}/{}".format(host, port, name) self.factory = WebSocketClientFactory(self.url, debug=True) + self.daemon = True def run(self): self.factory.protocol = MMClient @@ -54,15 +49,7 @@ def stop(self): def send_message(self, payload): return MMClient.send_message(payload) - -if __name__ == "__main__": - import time - conn = Connection("localhost", 9000, sys.argv[1]) - conn.daemon = True - conn.start() - msg_tmp = string.Template(test_msg) - while True: - msg = msg_tmp.substitute( - fr=sys.argv[1], to=sys.argv[2], t=time.time()) - conn.send_message(msg) - time.sleep(0.1) + def updates(self): + payloads = copy.deepcopy(MMClient.updates) + MMClient.updates = dict() + return payloads diff --git a/src/client/node.py b/src/client/node.py new file mode 100644 index 0000000..6b8b884 --- /dev/null +++ b/src/client/node.py @@ -0,0 +1,64 @@ + +import rospy +import sys +import time +import string +import publishermanager as pm +import json +from rospy_message_converter import message_converter as mc +from connection import Connection +from geometry_msgs.msg import Point + + +NODE_NAME = "jammi" + +class JammiNode(object): + + def __init__(self, host, port, name, topics): + self.host = host + self.port = port + self.name = name + self.conn = Connection(host, port, name) + self.conn.start() + self.subs = dict() + self.topics = topics + self.pub_man = pm.PublisherManager() + + def run(self): + for topic, msg_type in self.topics: + self.create_subscriber(topic, msg_type) + r = rospy.Rate(30) + while not rospy.is_shutdown(): + updates = self.conn.updates() + for v in updates.values(): + self.pub_man.publish(v) + r.sleep() + + + def create_subscriber(self, topic, msg_type): + namespace, msg_name = msg_type.split("/") + mod = __import__(namespace + ".msg") + msg_cls = getattr(mod.msg, msg_name) + cb = self.create_callback(topic, msg_type) + self.subs[topic] = rospy.Subscriber(topic, msg_cls, cb) + return self + + def create_callback(self, topic, msg_type): + def callback(msg): + data = dict() + data["to"] = "*" + data["from"] = self.name + data["topic"] = "/{}{}".format(self.name, topic) + data["type"] = msg_type + data["stamp"] = time.time() + data["msg"] = mc.convert_ros_message_to_dictionary(msg) + payload = json.dumps(data) + self.conn.send_message(payload) + return callback + + +if __name__ == "__main__": + rospy.init_node(sys.argv[1], anonymous=False) + topics = [("/state", "geometry_msgs/Point")] + jn = JammiNode("localhost", 9000, sys.argv[1], topics) + jn.run() diff --git a/src/client/publishermanager.py b/src/client/publishermanager.py new file mode 100644 index 0000000..eb4bce8 --- /dev/null +++ b/src/client/publishermanager.py @@ -0,0 +1,25 @@ +import rospy +import json +from rospy_message_converter import message_converter + + +class PublisherManager(object): + + def __init__(self): + self.pubs = dict() + + def create_msg(self, msg_dict, msg_type): + namespace, msg_name = msg_type.split("/") + mod = __import__(namespace + ".msg") + msg_cls = getattr(mod.msg, msg_name) + msg = message_converter.convert_dictionary_to_ros_message( + msg_type, msg_dict) + return msg, msg_cls + + def publish(self, payload): + data = json.loads(payload) + msg, msg_cls = self.create_msg(data["msg"], data["type"]) + topic = data["topic"] + if not topic in self.pubs.keys(): + self.pubs[topic] = rospy.Publisher(topic, msg_cls, queue_size=2) + self.pubs[topic].publish(msg) diff --git a/src/server/ws.py b/src/server/ws.py index 75db62f..f24a291 100644 --- a/src/server/ws.py +++ b/src/server/ws.py @@ -20,7 +20,8 @@ def onMessage(self, payload, is_binary): msg = json.loads(payload) if msg["to"] == "*": for name in common.clients.keys(): - common.get_client(msg["to"]).sendMessage(payload) + if name != msg["from"]: + common.get_client(name).sendMessage(payload) else: common.get_client(msg["to"]).sendMessage(payload) except KeyError: