diff --git a/launch/example.launch b/launch/example.launch index 4f9999c..63f7e6c 100644 --- a/launch/example.launch +++ b/launch/example.launch @@ -1,32 +1,27 @@ - - - - - - - + + - - + + - - + + - - + + publishing: - /state @@ -35,7 +30,7 @@ - geometry_msgs/Point - geometry_msgs/Point trusted: - - foo bar baz + - "*" - foo bar diff --git a/src/client/client_node.py b/src/client/client_node.py index 970eb12..54eaa38 100755 --- a/src/client/client_node.py +++ b/src/client/client_node.py @@ -35,7 +35,7 @@ def create_subscriber(self, topic, msg_type, trusted): mod = __import__(namespace + ".msg") msg_cls = getattr(mod.msg, msg_name) cb = self.create_callback(topic, msg_type, trusted) - self.subs[topic] = rospy.Subscriber(topic, msg_cls, cb) + self.subs[topic] = rospy.Subscriber(topic, msg_cls, cb, None, 1) return self def create_callback(self, topic, msg_type, trusted): diff --git a/src/client/connection.py b/src/client/connection.py index 62d43db..db84003 100644 --- a/src/client/connection.py +++ b/src/client/connection.py @@ -7,33 +7,52 @@ from twisted.internet import reactor from autobahn.twisted.websocket import WebSocketClientProtocol from autobahn.twisted.websocket import WebSocketClientFactory - +import rospy class MMClient(WebSocketClientProtocol): client = None updates = dict() + acknowledged = True + timer = threading.Timer def onConnect(self, reponse): MMClient.client = self + MMClient.acknowledged = True + MMClient.timer = threading.Timer def onMessage(self, payload, is_binary): if not is_binary: data = json.loads(payload) MMClient.updates[data["topic"]] = data else: - decompressed = zlib.decompress(payload) - size = struct.unpack('=I', decompressed[:4]) - frmt = "%ds" % size[0] - unpacked = struct.unpack('=I' + frmt, decompressed) - data = json.loads(unpacked[1]) - MMClient.updates[data["topic"]] = data + if len(payload) == 1: + MMClient.acknowledged = True + MMClient.timer.cancel() + else: + decompressed = zlib.decompress(payload) + size = struct.unpack('=I', decompressed[:4]) + frmt = "%ds" % size[0] + unpacked = struct.unpack('=I' + frmt, decompressed) + data = json.loads(unpacked[1]) + MMClient.updates[data["topic"]] = data + + def onClose(self, wasClean, code, reason): + rospy.logwarn("WebSocket connection closed: {0}".format(reason)) + + @staticmethod + def timeout(): + MMClient.acknowledged = True @staticmethod def send_message(payload, is_binary): if not MMClient.client is None: - MMClient.client.sendMessage(payload, is_binary) - + rospy.loginfo(MMClient.acknowledged) + if MMClient.acknowledged: + MMClient.acknowledged = False + MMClient.client.sendMessage(payload, is_binary) + MMClient.timer = threading.Timer(1, MMClient.timeout) + MMClient.timer.start() class Connection(threading.Thread): def __init__(self, host, port, name):