diff --git a/distro/travis/install_deps.sh b/distro/travis/install_deps.sh index 6b7f31db3..51e601126 100755 --- a/distro/travis/install_deps.sh +++ b/distro/travis/install_deps.sh @@ -20,6 +20,7 @@ install_ubuntu_deps_common() python-numpy \ python-scipy \ python-yaml \ + python-zmq \ wget \ xvfb @@ -71,8 +72,9 @@ install_osx_deps() brew install glib # for lcm brew ls --versions python || brew install python brew ls --versions numpy || brew install numpy || echo "error on brew install numpy" + brew install zeromq - pip install coverage lxml PyYAML Sphinx sphinx_rtd_theme + pip install coverage lxml PyYAML Sphinx sphinx_rtd_theme pyzmq } diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 308165204..d1fce6a9a 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -173,6 +173,8 @@ set(python_files director/thirdparty/naming.py director/thirdparty/pysdf.py director/thirdparty/toposort.py + director/thirdparty/umsgpack.py + director/thirdparty/msgpack_numpy.py urdf_parser_py/__init__.py urdf_parser_py/sdf.py diff --git a/src/python/director/drakevisualizerapp.py b/src/python/director/drakevisualizerapp.py index e4fb58803..aecd243a9 100644 --- a/src/python/director/drakevisualizerapp.py +++ b/src/python/director/drakevisualizerapp.py @@ -23,11 +23,13 @@ def main(globalsDict=None): fact.register(mainwindowapp.MainWindowAppFactory) fact.register(mainwindowapp.MainWindowPanelFactory) + args = drcargs.args() options = fact.getDefaultOptions() fact.setDependentOptions(options, - useTreeViewer=HAVE_LCMRL, - useDrakeVisualizer=True, - useLCMGLRenderer=True) + useLCMTreeViewer=HAVE_LCMRL and args.treeviewer_lcm, + useZMQTreeViewer=args.treeviewer_zmq_url, + useDrakeVisualizer=args.drakevisualizer_lcm, + useLCMGLRenderer=args.lcmgl_renderer) fields = fact.construct( options=options, diff --git a/src/python/director/drcargs.py b/src/python/director/drcargs.py index 6bcccb8f9..0baa8ff8d 100644 --- a/src/python/director/drcargs.py +++ b/src/python/director/drcargs.py @@ -170,6 +170,24 @@ def addDefaultArgs(self, parser): default=[], action='append', metavar='filename', help='python scripts to run at startup') + parser.add_argument('--treeviewer-zmq-url', type=str, + dest='treeviewer_zmq_url', default='tcp://*:57370') + + tvlcm_parser = parser.add_mutually_exclusive_group(required=False) + tvlcm_parser.add_argument('--treeviewer-lcm', action='store_true', dest='treeviewer_lcm') + tvlcm_parser.add_argument('--no-treeviewer-lcm', action='store_false', dest='treeviewer_lcm') + parser.set_defaults(treeviewer_lcm=True) + + dvlcm_parser = parser.add_mutually_exclusive_group(required=False) + dvlcm_parser.add_argument('--drakevisualizer-lcm', action='store_true', dest='drakevisualizer_lcm') + dvlcm_parser.add_argument('--no-drakevisualizer-lcm', action='store_false', dest='drakevisualizer_lcm') + parser.set_defaults(drakevisualizer_lcm=True) + + lcmgl_parser = parser.add_mutually_exclusive_group(required=False) + lcmgl_parser.add_argument('--lcmgl-renderer', action='store_true', dest='lcmgl_renderer') + lcmgl_parser.add_argument('--no-lcmgl-renderer', action='store_false', dest='lcmgl_renderer') + parser.set_defaults(lcmgl_renderer=True) + _argParser = None def getGlobalArgParser(): diff --git a/src/python/director/mainwindowapp.py b/src/python/director/mainwindowapp.py index 40288fcfe..ddd331bc5 100644 --- a/src/python/director/mainwindowapp.py +++ b/src/python/director/mainwindowapp.py @@ -18,6 +18,7 @@ def __init__(self): self.mainWindow = QtGui.QMainWindow() self.mainWindow.resize(768 * (16/9.0), 768) + self.mainWindow.setAttribute(QtCore.Qt.WA_ShowWithoutActivating) self.settings = QtCore.QSettings() self.fileMenu = self.mainWindow.menuBar().addMenu('&File') @@ -350,14 +351,16 @@ def getComponents(self): 'OutputConsole' : ['MainWindow'], 'UndoRedo' : ['MainWindow'], 'DrakeVisualizer' : ['MainWindow'], - 'TreeViewer' : ['MainWindow'], + 'LCMTreeViewer' : ['MainWindow'], + 'ZMQTreeViewer' : ['MainWindow'], 'LCMGLRenderer' : ['MainWindow']} # these components depend on lcm and lcmgl # so they are disabled by default disabledComponents = [ 'DrakeVisualizer', - 'TreeViewer', + 'LCMTreeViewer', + 'ZMQTreeViewer', 'LCMGLRenderer'] return components, disabledComponents @@ -460,15 +463,27 @@ def initDrakeVisualizer(self, fields): drakeVisualizer=drakeVisualizer ) - def initTreeViewer(self, fields): + def initLCMTreeViewer(self, fields): from director import treeviewer - treeViewer = treeviewer.TreeViewer(fields.view) + treeViewer = treeviewer.LCMTreeViewer(fields.view) applogic.MenuActionToggleHelper('Tools', treeViewer.name, treeViewer.isEnabled, treeViewer.setEnabled) return FieldContainer( - treeViewer=treeViewer + lcmTreeViewer=treeViewer + ) + + def initZMQTreeViewer(self, fields): + + from director import treeviewer + args = drcargs.args() + treeViewer = treeviewer.ZMQTreeViewer(fields.view, zmqUrl=args.treeviewer_zmq_url) + + applogic.MenuActionToggleHelper('Tools', treeViewer.name, treeViewer.isEnabled, treeViewer.setEnabled) + + return FieldContainer( + zmqTreeViewer=treeViewer ) def initLCMGLRenderer(self, fields): diff --git a/src/python/director/taskrunner.py b/src/python/director/taskrunner.py index e00f64e47..98b6d634a 100644 --- a/src/python/director/taskrunner.py +++ b/src/python/director/taskrunner.py @@ -56,6 +56,7 @@ def callOnMain(self, func, *args, **kwargs): def callOnThread(self, func, *args, **kwargs): t = Thread(target=lambda: func(*args, **kwargs)) + t.daemon = True # daemon kwarg doesn't exist in python 2.7 self.threads.append(t) t.start() self.timer.start() diff --git a/src/python/director/thirdparty/msgpack_numpy.py b/src/python/director/thirdparty/msgpack_numpy.py new file mode 100644 index 000000000..1c3abc8e9 --- /dev/null +++ b/src/python/director/thirdparty/msgpack_numpy.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python + +""" +Support for serialization of numpy data types with msgpack. +""" + +# Copyright (c) 2013-2017, Lev E. Givon. +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are +# met: + +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Lev E. Givon nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. + +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# +# Modified by Robin Deits (2017): +# * Removed all explicit references to msgpack so that this can be used +# with umsgpack instead + + +import sys + +import numpy as np + +def encode(obj, chain=None): + """ + Data encoder for serializing numpy data types. + """ + + if isinstance(obj, np.ndarray): + # If the dtype is structured, store the interface description; + # otherwise, store the corresponding array protocol type string: + if obj.dtype.kind == 'V': + kind = b'V' + descr = obj.dtype.descr + else: + kind = b'' + descr = obj.dtype.str + return {b'nd': True, + b'type': descr, + b'kind': kind, + b'shape': obj.shape, + b'data': obj.tobytes()} + elif isinstance(obj, (np.bool_, np.number)): + return {b'nd': False, + b'type': obj.dtype.str, + b'data': obj.tobytes()} + elif isinstance(obj, complex): + return {b'complex': True, + b'data': obj.__repr__()} + else: + return obj if chain is None else chain(obj) + +def tostr(x): + if sys.version_info >= (3, 0): + if isinstance(x, bytes): + return x.decode() + else: + return str(x) + else: + return x + +def decode(obj, chain=None): + """ + Decoder for deserializing numpy data types. + """ + + try: + if b'nd' in obj: + if obj[b'nd'] is True: + + # Check if b'kind' is in obj to enable decoding of data + # serialized with older versions (#20): + if b'kind' in obj and obj[b'kind'] == b'V': + descr = [tuple(tostr(t) if type(t) is bytes else t for t in d) \ + for d in obj[b'type']] + else: + descr = obj[b'type'] + return np.fromstring(obj[b'data'], + dtype=np.dtype(descr)).reshape(obj[b'shape']) + else: + descr = obj[b'type'] + return np.fromstring(obj[b'data'], + dtype=np.dtype(descr))[0] + elif b'complex' in obj: + return complex(tostr(obj[b'data'])) + else: + return obj if chain is None else chain(obj) + except KeyError: + return obj if chain is None else chain(obj) diff --git a/src/python/director/thirdparty/umsgpack.py b/src/python/director/thirdparty/umsgpack.py new file mode 100644 index 000000000..cd7a2037e --- /dev/null +++ b/src/python/director/thirdparty/umsgpack.py @@ -0,0 +1,1057 @@ +# u-msgpack-python v2.4.1 - v at sergeev.io +# https://github.com/vsergeev/u-msgpack-python +# +# u-msgpack-python is a lightweight MessagePack serializer and deserializer +# module, compatible with both Python 2 and 3, as well CPython and PyPy +# implementations of Python. u-msgpack-python is fully compliant with the +# latest MessagePack specification.com/msgpack/msgpack/blob/master/spec.md). In +# particular, it supports the new binary, UTF-8 string, and application ext +# types. +# +# MIT License +# +# Copyright (c) 2013-2016 vsergeev / Ivan (Vanya) A. Sergeev +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +# +""" +u-msgpack-python v2.4.1 - v at sergeev.io +https://github.com/vsergeev/u-msgpack-python + +u-msgpack-python is a lightweight MessagePack serializer and deserializer +module, compatible with both Python 2 and 3, as well CPython and PyPy +implementations of Python. u-msgpack-python is fully compliant with the +latest MessagePack specification.com/msgpack/msgpack/blob/master/spec.md). In +particular, it supports the new binary, UTF-8 string, and application ext +types. + +License: MIT +""" +import struct +import collections +import sys +import io + +__version__ = "2.4.1" +"Module version string" + +version = (2, 4, 1) +"Module version tuple" + + +############################################################################## +# Ext Class +############################################################################## + +# Extension type for application-defined types and data +class Ext: + """ + The Ext class facilitates creating a serializable extension object to store + an application-defined type and data byte array. + """ + + def __init__(self, type, data): + """ + Construct a new Ext object. + + Args: + type: application-defined type integer from 0 to 127 + data: application-defined data byte array + + Raises: + TypeError: + Specified ext type is outside of 0 to 127 range. + + Example: + >>> foo = umsgpack.Ext(0x05, b"\x01\x02\x03") + >>> umsgpack.packb({u"special stuff": foo, u"awesome": True}) + '\x82\xa7awesome\xc3\xadspecial stuff\xc7\x03\x05\x01\x02\x03' + >>> bar = umsgpack.unpackb(_) + >>> print(bar["special stuff"]) + Ext Object (Type: 0x05, Data: 01 02 03) + >>> + """ + # Application ext type should be 0 <= type <= 127 + if not isinstance(type, int) or not (type >= 0 and type <= 127): + raise TypeError("ext type out of range") + # Check data is type bytes + elif sys.version_info[0] == 3 and not isinstance(data, bytes): + raise TypeError("ext data is not type \'bytes\'") + elif sys.version_info[0] == 2 and not isinstance(data, str): + raise TypeError("ext data is not type \'str\'") + self.type = type + self.data = data + + def __eq__(self, other): + """ + Compare this Ext object with another for equality. + """ + return (isinstance(other, self.__class__) and + self.type == other.type and + self.data == other.data) + + def __ne__(self, other): + """ + Compare this Ext object with another for inequality. + """ + return not self.__eq__(other) + + def __str__(self): + """ + String representation of this Ext object. + """ + s = "Ext Object (Type: 0x%02x, Data: " % self.type + s += " ".join(["0x%02x" % ord(self.data[i:i + 1]) + for i in xrange(min(len(self.data), 8))]) + if len(self.data) > 8: + s += " ..." + s += ")" + return s + + def __hash__(self): + """ + Provide a hash of this Ext object. + """ + return hash((self.type, self.data)) + + +class InvalidString(bytes): + """Subclass of bytes to hold invalid UTF-8 strings.""" + pass + +############################################################################## +# Exceptions +############################################################################## + + +# Base Exception classes +class PackException(Exception): + "Base class for exceptions encountered during packing." + pass + + +class UnpackException(Exception): + "Base class for exceptions encountered during unpacking." + pass + + +# Packing error +class UnsupportedTypeException(PackException): + "Object type not supported for packing." + pass + + +# Unpacking error +class InsufficientDataException(UnpackException): + "Insufficient data to unpack the serialized object." + pass + + +class InvalidStringException(UnpackException): + "Invalid UTF-8 string encountered during unpacking." + pass + + +class ReservedCodeException(UnpackException): + "Reserved code encountered during unpacking." + pass + + +class UnhashableKeyException(UnpackException): + """ + Unhashable key encountered during map unpacking. + The serialized map cannot be deserialized into a Python dictionary. + """ + pass + + +class DuplicateKeyException(UnpackException): + "Duplicate key encountered during map unpacking." + pass + + +# Backwards compatibility +KeyNotPrimitiveException = UnhashableKeyException +KeyDuplicateException = DuplicateKeyException + +############################################################################# +# Exported Functions and Glob +############################################################################# + +# Exported functions and variables, set up in __init() +pack = None +packb = None +unpack = None +unpackb = None +dump = None +dumps = None +load = None +loads = None + +compatibility = False +""" +Compatibility mode boolean. + +When compatibility mode is enabled, u-msgpack-python will serialize both +unicode strings and bytes into the old "raw" msgpack type, and deserialize the +"raw" msgpack type into bytes. This provides backwards compatibility with the +old MessagePack specification. + +Example: +>>> umsgpack.compatibility = True +>>> +>>> umsgpack.packb([u"some string", b"some bytes"]) +b'\x92\xabsome string\xaasome bytes' +>>> umsgpack.unpackb(_) +[b'some string', b'some bytes'] +>>> +""" + +############################################################################## +# Packing +############################################################################## + +# You may notice struct.pack("B", obj) instead of the simpler chr(obj) in the +# code below. This is to allow for seamless Python 2 and 3 compatibility, as +# chr(obj) has a str return type instead of bytes in Python 3, and +# struct.pack(...) has the right return type in both versions. + + +def _pack_integer(obj, fp, options): + if obj < 0: + if obj >= -32: + fp.write(struct.pack("b", obj)) + elif obj >= -2**(8 - 1): + fp.write(b"\xd0" + struct.pack("b", obj)) + elif obj >= -2**(16 - 1): + fp.write(b"\xd1" + struct.pack(">h", obj)) + elif obj >= -2**(32 - 1): + fp.write(b"\xd2" + struct.pack(">i", obj)) + elif obj >= -2**(64 - 1): + fp.write(b"\xd3" + struct.pack(">q", obj)) + else: + raise UnsupportedTypeException("huge signed int") + else: + if obj <= 127: + fp.write(struct.pack("B", obj)) + elif obj <= 2**8 - 1: + fp.write(b"\xcc" + struct.pack("B", obj)) + elif obj <= 2**16 - 1: + fp.write(b"\xcd" + struct.pack(">H", obj)) + elif obj <= 2**32 - 1: + fp.write(b"\xce" + struct.pack(">I", obj)) + elif obj <= 2**64 - 1: + fp.write(b"\xcf" + struct.pack(">Q", obj)) + else: + raise UnsupportedTypeException("huge unsigned int") + + +def _pack_nil(obj, fp, options): + fp.write(b"\xc0") + + +def _pack_boolean(obj, fp, options): + fp.write(b"\xc3" if obj else b"\xc2") + + +def _pack_float(obj, fp, options): + float_precision = options.get('force_float_precision', _float_precision) + + if float_precision == "double": + fp.write(b"\xcb" + struct.pack(">d", obj)) + elif float_precision == "single": + fp.write(b"\xca" + struct.pack(">f", obj)) + else: + raise ValueError("invalid float precision") + + +def _pack_string(obj, fp, options): + obj = obj.encode('utf-8') + if len(obj) <= 31: + fp.write(struct.pack("B", 0xa0 | len(obj)) + obj) + elif len(obj) <= 2**8 - 1: + fp.write(b"\xd9" + struct.pack("B", len(obj)) + obj) + elif len(obj) <= 2**16 - 1: + fp.write(b"\xda" + struct.pack(">H", len(obj)) + obj) + elif len(obj) <= 2**32 - 1: + fp.write(b"\xdb" + struct.pack(">I", len(obj)) + obj) + else: + raise UnsupportedTypeException("huge string") + + +def _pack_binary(obj, fp, options): + if len(obj) <= 2**8 - 1: + fp.write(b"\xc4" + struct.pack("B", len(obj)) + obj) + elif len(obj) <= 2**16 - 1: + fp.write(b"\xc5" + struct.pack(">H", len(obj)) + obj) + elif len(obj) <= 2**32 - 1: + fp.write(b"\xc6" + struct.pack(">I", len(obj)) + obj) + else: + raise UnsupportedTypeException("huge binary string") + + +def _pack_oldspec_raw(obj, fp, options): + if len(obj) <= 31: + fp.write(struct.pack("B", 0xa0 | len(obj)) + obj) + elif len(obj) <= 2**16 - 1: + fp.write(b"\xda" + struct.pack(">H", len(obj)) + obj) + elif len(obj) <= 2**32 - 1: + fp.write(b"\xdb" + struct.pack(">I", len(obj)) + obj) + else: + raise UnsupportedTypeException("huge raw string") + + +def _pack_ext(obj, fp, options): + if len(obj.data) == 1: + fp.write(b"\xd4" + struct.pack("B", obj.type & 0xff) + obj.data) + elif len(obj.data) == 2: + fp.write(b"\xd5" + struct.pack("B", obj.type & 0xff) + obj.data) + elif len(obj.data) == 4: + fp.write(b"\xd6" + struct.pack("B", obj.type & 0xff) + obj.data) + elif len(obj.data) == 8: + fp.write(b"\xd7" + struct.pack("B", obj.type & 0xff) + obj.data) + elif len(obj.data) == 16: + fp.write(b"\xd8" + struct.pack("B", obj.type & 0xff) + obj.data) + elif len(obj.data) <= 2**8 - 1: + fp.write(b"\xc7" + + struct.pack("BB", len(obj.data), obj.type & 0xff) + obj.data) + elif len(obj.data) <= 2**16 - 1: + fp.write(b"\xc8" + + struct.pack(">HB", len(obj.data), obj.type & 0xff) + obj.data) + elif len(obj.data) <= 2**32 - 1: + fp.write(b"\xc9" + + struct.pack(">IB", len(obj.data), obj.type & 0xff) + obj.data) + else: + raise UnsupportedTypeException("huge ext data") + + +def _pack_array(obj, fp, options): + if len(obj) <= 15: + fp.write(struct.pack("B", 0x90 | len(obj))) + elif len(obj) <= 2**16 - 1: + fp.write(b"\xdc" + struct.pack(">H", len(obj))) + elif len(obj) <= 2**32 - 1: + fp.write(b"\xdd" + struct.pack(">I", len(obj))) + else: + raise UnsupportedTypeException("huge array") + + for e in obj: + pack(e, fp, **options) + + +def _pack_map(obj, fp, options): + if len(obj) <= 15: + fp.write(struct.pack("B", 0x80 | len(obj))) + elif len(obj) <= 2**16 - 1: + fp.write(b"\xde" + struct.pack(">H", len(obj))) + elif len(obj) <= 2**32 - 1: + fp.write(b"\xdf" + struct.pack(">I", len(obj))) + else: + raise UnsupportedTypeException("huge array") + + for k, v in obj.items(): + pack(k, fp, **options) + pack(v, fp, **options) + +######################################## + + +# Pack for Python 2, with 'unicode' type, 'str' type, and 'long' type +def _pack2(obj, fp, **options): + """ + Serialize a Python object into MessagePack bytes. + + Args: + obj: a Python object + fp: a .write()-supporting file-like object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping a custom type + to a callable that packs an instance of the type + into an Ext object + force_float_precision (str): "single" to force packing floats as + IEEE-754 single-precision floats, + "double" to force packing floats as + IEEE-754 double-precision floats. + + Returns: + None. + + Raises: + UnsupportedType(PackException): + Object type not supported for packing. + + Example: + >>> f = open('test.bin', 'wb') + >>> umsgpack.pack({u"compact": True, u"schema": 0}, f) + >>> + """ + global compatibility + + ext_handlers = options.get("ext_handlers") + + if obj is None: + _pack_nil(obj, fp, options) + elif ext_handlers and obj.__class__ in ext_handlers: + _pack_ext(ext_handlers[obj.__class__](obj), fp, options) + elif isinstance(obj, bool): + _pack_boolean(obj, fp, options) + elif isinstance(obj, int) or isinstance(obj, long): + _pack_integer(obj, fp, options) + elif isinstance(obj, float): + _pack_float(obj, fp, options) + elif compatibility and isinstance(obj, unicode): + _pack_oldspec_raw(bytes(obj), fp, options) + elif compatibility and isinstance(obj, bytes): + _pack_oldspec_raw(obj, fp, options) + elif isinstance(obj, unicode): + _pack_string(obj, fp, options) + elif isinstance(obj, str): + _pack_binary(obj, fp, options) + elif isinstance(obj, list) or isinstance(obj, tuple): + _pack_array(obj, fp, options) + elif isinstance(obj, dict): + _pack_map(obj, fp, options) + elif isinstance(obj, Ext): + _pack_ext(obj, fp, options) + elif ext_handlers: + # Linear search for superclass + t = next((t for t in ext_handlers.keys() if isinstance(obj, t)), None) + if t: + _pack_ext(ext_handlers[t](obj), fp, options) + else: + raise UnsupportedTypeException( + "unsupported type: %s" % str(type(obj))) + else: + raise UnsupportedTypeException("unsupported type: %s" % str(type(obj))) + + +# Pack for Python 3, with unicode 'str' type, 'bytes' type, and no 'long' type +def _pack3(obj, fp, **options): + """ + Serialize a Python object into MessagePack bytes. + + Args: + obj: a Python object + fp: a .write()-supporting file-like object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping a custom type + to a callable that packs an instance of the type + into an Ext object + force_float_precision (str): "single" to force packing floats as + IEEE-754 single-precision floats, + "double" to force packing floats as + IEEE-754 double-precision floats. + + Returns: + None. + + Raises: + UnsupportedType(PackException): + Object type not supported for packing. + + Example: + >>> f = open('test.bin', 'wb') + >>> umsgpack.pack({u"compact": True, u"schema": 0}, f) + >>> + """ + global compatibility + + ext_handlers = options.get("ext_handlers") + + if obj is None: + _pack_nil(obj, fp, options) + elif ext_handlers and obj.__class__ in ext_handlers: + _pack_ext(ext_handlers[obj.__class__](obj), fp, options) + elif isinstance(obj, bool): + _pack_boolean(obj, fp, options) + elif isinstance(obj, int): + _pack_integer(obj, fp, options) + elif isinstance(obj, float): + _pack_float(obj, fp, options) + elif compatibility and isinstance(obj, str): + _pack_oldspec_raw(obj.encode('utf-8'), fp, options) + elif compatibility and isinstance(obj, bytes): + _pack_oldspec_raw(obj, fp, options) + elif isinstance(obj, str): + _pack_string(obj, fp, options) + elif isinstance(obj, bytes): + _pack_binary(obj, fp, options) + elif isinstance(obj, list) or isinstance(obj, tuple): + _pack_array(obj, fp, options) + elif isinstance(obj, dict): + _pack_map(obj, fp, options) + elif isinstance(obj, Ext): + _pack_ext(obj, fp, options) + elif ext_handlers: + # Linear search for superclass + t = next((t for t in ext_handlers.keys() if isinstance(obj, t)), None) + if t: + _pack_ext(ext_handlers[t](obj), fp, options) + else: + raise UnsupportedTypeException( + "unsupported type: %s" % str(type(obj))) + else: + raise UnsupportedTypeException( + "unsupported type: %s" % str(type(obj))) + + +def _packb2(obj, **options): + """ + Serialize a Python object into MessagePack bytes. + + Args: + obj: a Python object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping a custom type + to a callable that packs an instance of the type + into an Ext object + force_float_precision (str): "single" to force packing floats as + IEEE-754 single-precision floats, + "double" to force packing floats as + IEEE-754 double-precision floats. + + Returns: + A 'str' containing serialized MessagePack bytes. + + Raises: + UnsupportedType(PackException): + Object type not supported for packing. + + Example: + >>> umsgpack.packb({u"compact": True, u"schema": 0}) + '\x82\xa7compact\xc3\xa6schema\x00' + >>> + """ + fp = io.BytesIO() + _pack2(obj, fp, **options) + return fp.getvalue() + + +def _packb3(obj, **options): + """ + Serialize a Python object into MessagePack bytes. + + Args: + obj: a Python object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping a custom type + to a callable that packs an instance of the type + into an Ext object + force_float_precision (str): "single" to force packing floats as + IEEE-754 single-precision floats, + "double" to force packing floats as + IEEE-754 double-precision floats. + + Returns: + A 'bytes' containing serialized MessagePack bytes. + + Raises: + UnsupportedType(PackException): + Object type not supported for packing. + + Example: + >>> umsgpack.packb({u"compact": True, u"schema": 0}) + b'\x82\xa7compact\xc3\xa6schema\x00' + >>> + """ + fp = io.BytesIO() + _pack3(obj, fp, **options) + return fp.getvalue() + +############################################################################# +# Unpacking +############################################################################# + + +def _read_except(fp, n): + data = fp.read(n) + if len(data) < n: + raise InsufficientDataException() + return data + + +def _unpack_integer(code, fp, options): + if (ord(code) & 0xe0) == 0xe0: + return struct.unpack("b", code)[0] + elif code == b'\xd0': + return struct.unpack("b", _read_except(fp, 1))[0] + elif code == b'\xd1': + return struct.unpack(">h", _read_except(fp, 2))[0] + elif code == b'\xd2': + return struct.unpack(">i", _read_except(fp, 4))[0] + elif code == b'\xd3': + return struct.unpack(">q", _read_except(fp, 8))[0] + elif (ord(code) & 0x80) == 0x00: + return struct.unpack("B", code)[0] + elif code == b'\xcc': + return struct.unpack("B", _read_except(fp, 1))[0] + elif code == b'\xcd': + return struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xce': + return struct.unpack(">I", _read_except(fp, 4))[0] + elif code == b'\xcf': + return struct.unpack(">Q", _read_except(fp, 8))[0] + raise Exception("logic error, not int: 0x%02x" % ord(code)) + + +def _unpack_reserved(code, fp, options): + if code == b'\xc1': + raise ReservedCodeException( + "encountered reserved code: 0x%02x" % ord(code)) + raise Exception( + "logic error, not reserved code: 0x%02x" % ord(code)) + + +def _unpack_nil(code, fp, options): + if code == b'\xc0': + return None + raise Exception("logic error, not nil: 0x%02x" % ord(code)) + + +def _unpack_boolean(code, fp, options): + if code == b'\xc2': + return False + elif code == b'\xc3': + return True + raise Exception("logic error, not boolean: 0x%02x" % ord(code)) + + +def _unpack_float(code, fp, options): + if code == b'\xca': + return struct.unpack(">f", _read_except(fp, 4))[0] + elif code == b'\xcb': + return struct.unpack(">d", _read_except(fp, 8))[0] + raise Exception("logic error, not float: 0x%02x" % ord(code)) + + +def _unpack_string(code, fp, options): + if (ord(code) & 0xe0) == 0xa0: + length = ord(code) & ~0xe0 + elif code == b'\xd9': + length = struct.unpack("B", _read_except(fp, 1))[0] + elif code == b'\xda': + length = struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xdb': + length = struct.unpack(">I", _read_except(fp, 4))[0] + else: + raise Exception("logic error, not string: 0x%02x" % ord(code)) + + # Always return raw bytes in compatibility mode + global compatibility + if compatibility: + return _read_except(fp, length) + + data = _read_except(fp, length) + try: + return bytes.decode(data, 'utf-8') + except UnicodeDecodeError: + if options.get("allow_invalid_utf8"): + return InvalidString(data) + raise InvalidStringException("unpacked string is invalid utf-8") + + +def _unpack_binary(code, fp, options): + if code == b'\xc4': + length = struct.unpack("B", _read_except(fp, 1))[0] + elif code == b'\xc5': + length = struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xc6': + length = struct.unpack(">I", _read_except(fp, 4))[0] + else: + raise Exception("logic error, not binary: 0x%02x" % ord(code)) + + return _read_except(fp, length) + + +def _unpack_ext(code, fp, options): + if code == b'\xd4': + length = 1 + elif code == b'\xd5': + length = 2 + elif code == b'\xd6': + length = 4 + elif code == b'\xd7': + length = 8 + elif code == b'\xd8': + length = 16 + elif code == b'\xc7': + length = struct.unpack("B", _read_except(fp, 1))[0] + elif code == b'\xc8': + length = struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xc9': + length = struct.unpack(">I", _read_except(fp, 4))[0] + else: + raise Exception("logic error, not ext: 0x%02x" % ord(code)) + + ext = Ext(ord(_read_except(fp, 1)), _read_except(fp, length)) + + # Unpack with ext handler, if we have one + ext_handlers = options.get("ext_handlers") + if ext_handlers and ext.type in ext_handlers: + ext = ext_handlers[ext.type](ext) + + return ext + + +def _unpack_array(code, fp, options): + if (ord(code) & 0xf0) == 0x90: + length = (ord(code) & ~0xf0) + elif code == b'\xdc': + length = struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xdd': + length = struct.unpack(">I", _read_except(fp, 4))[0] + else: + raise Exception("logic error, not array: 0x%02x" % ord(code)) + + return [_unpack(fp, options) for i in xrange(length)] + + +def _deep_list_to_tuple(obj): + if isinstance(obj, list): + return tuple([_deep_list_to_tuple(e) for e in obj]) + return obj + + +def _unpack_map(code, fp, options): + if (ord(code) & 0xf0) == 0x80: + length = (ord(code) & ~0xf0) + elif code == b'\xde': + length = struct.unpack(">H", _read_except(fp, 2))[0] + elif code == b'\xdf': + length = struct.unpack(">I", _read_except(fp, 4))[0] + else: + raise Exception("logic error, not map: 0x%02x" % ord(code)) + + d = {} if not options.get('use_ordered_dict') \ + else collections.OrderedDict() + for _ in xrange(length): + # Unpack key + k = _unpack(fp, options) + + if isinstance(k, list): + # Attempt to convert list into a hashable tuple + k = _deep_list_to_tuple(k) + elif not isinstance(k, collections.Hashable): + raise UnhashableKeyException( + "encountered unhashable key: %s, %s" % (str(k), str(type(k)))) + elif k in d: + raise DuplicateKeyException( + "encountered duplicate key: %s, %s" % (str(k), str(type(k)))) + + # Unpack value + v = _unpack(fp, options) + + try: + d[k] = v + except TypeError: + raise UnhashableKeyException( + "encountered unhashable key: %s" % str(k)) + return d + + +def _unpack(fp, options): + code = _read_except(fp, 1) + return _unpack_dispatch_table[code](code, fp, options) + +######################################## + + +def _unpack2(fp, **options): + """ + Deserialize MessagePack bytes into a Python object. + + Args: + fp: a .read()-supporting file-like object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping integer Ext + type to a callable that unpacks an instance of + Ext into an object + use_ordered_dict (bool): unpack maps into OrderedDict, instead of + unordered dict (default False) + allow_invalid_utf8 (bool): unpack invalid strings into instances of + InvalidString, for access to the bytes + (default False) + + Returns: + A Python object. + + Raises: + InsufficientDataException(UnpackException): + Insufficient data to unpack the serialized object. + InvalidStringException(UnpackException): + Invalid UTF-8 string encountered during unpacking. + ReservedCodeException(UnpackException): + Reserved code encountered during unpacking. + UnhashableKeyException(UnpackException): + Unhashable key encountered during map unpacking. + The serialized map cannot be deserialized into a Python dictionary. + DuplicateKeyException(UnpackException): + Duplicate key encountered during map unpacking. + + Example: + >>> f = open('test.bin', 'rb') + >>> umsgpack.unpackb(f) + {u'compact': True, u'schema': 0} + >>> + """ + return _unpack(fp, options) + + +def _unpack3(fp, **options): + """ + Deserialize MessagePack bytes into a Python object. + + Args: + fp: a .read()-supporting file-like object + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping integer Ext + type to a callable that unpacks an instance of + Ext into an object + use_ordered_dict (bool): unpack maps into OrderedDict, instead of + unordered dict (default False) + allow_invalid_utf8 (bool): unpack invalid strings into instances of + InvalidString, for access to the bytes + (default False) + + Returns: + A Python object. + + Raises: + InsufficientDataException(UnpackException): + Insufficient data to unpack the serialized object. + InvalidStringException(UnpackException): + Invalid UTF-8 string encountered during unpacking. + ReservedCodeException(UnpackException): + Reserved code encountered during unpacking. + UnhashableKeyException(UnpackException): + Unhashable key encountered during map unpacking. + The serialized map cannot be deserialized into a Python dictionary. + DuplicateKeyException(UnpackException): + Duplicate key encountered during map unpacking. + + Example: + >>> f = open('test.bin', 'rb') + >>> umsgpack.unpackb(f) + {'compact': True, 'schema': 0} + >>> + """ + return _unpack(fp, options) + + +# For Python 2, expects a str object +def _unpackb2(s, **options): + """ + Deserialize MessagePack bytes into a Python object. + + Args: + s: a 'str' or 'bytearray' containing serialized MessagePack bytes + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping integer Ext + type to a callable that unpacks an instance of + Ext into an object + use_ordered_dict (bool): unpack maps into OrderedDict, instead of + unordered dict (default False) + allow_invalid_utf8 (bool): unpack invalid strings into instances of + InvalidString, for access to the bytes + (default False) + + Returns: + A Python object. + + Raises: + TypeError: + Packed data type is neither 'str' nor 'bytearray'. + InsufficientDataException(UnpackException): + Insufficient data to unpack the serialized object. + InvalidStringException(UnpackException): + Invalid UTF-8 string encountered during unpacking. + ReservedCodeException(UnpackException): + Reserved code encountered during unpacking. + UnhashableKeyException(UnpackException): + Unhashable key encountered during map unpacking. + The serialized map cannot be deserialized into a Python dictionary. + DuplicateKeyException(UnpackException): + Duplicate key encountered during map unpacking. + + Example: + >>> umsgpack.unpackb(b'\x82\xa7compact\xc3\xa6schema\x00') + {u'compact': True, u'schema': 0} + >>> + """ + if not isinstance(s, (str, bytearray)): + raise TypeError("packed data must be type 'str' or 'bytearray'") + return _unpack(io.BytesIO(s), options) + + +# For Python 3, expects a bytes object +def _unpackb3(s, **options): + """ + Deserialize MessagePack bytes into a Python object. + + Args: + s: a 'bytes' or 'bytearray' containing serialized MessagePack bytes + + Kwargs: + ext_handlers (dict): dictionary of Ext handlers, mapping integer Ext + type to a callable that unpacks an instance of + Ext into an object + use_ordered_dict (bool): unpack maps into OrderedDict, instead of + unordered dict (default False) + allow_invalid_utf8 (bool): unpack invalid strings into instances of + InvalidString, for access to the bytes + (default False) + + Returns: + A Python object. + + Raises: + TypeError: + Packed data type is neither 'bytes' nor 'bytearray'. + InsufficientDataException(UnpackException): + Insufficient data to unpack the serialized object. + InvalidStringException(UnpackException): + Invalid UTF-8 string encountered during unpacking. + ReservedCodeException(UnpackException): + Reserved code encountered during unpacking. + UnhashableKeyException(UnpackException): + Unhashable key encountered during map unpacking. + The serialized map cannot be deserialized into a Python dictionary. + DuplicateKeyException(UnpackException): + Duplicate key encountered during map unpacking. + + Example: + >>> umsgpack.unpackb(b'\x82\xa7compact\xc3\xa6schema\x00') + {'compact': True, 'schema': 0} + >>> + """ + if not isinstance(s, (bytes, bytearray)): + raise TypeError("packed data must be type 'bytes' or 'bytearray'") + return _unpack(io.BytesIO(s), options) + +############################################################################# +# Module Initialization +############################################################################# + + +def __init(): + global pack + global packb + global unpack + global unpackb + global dump + global dumps + global load + global loads + global compatibility + global _float_precision + global _unpack_dispatch_table + global xrange + + # Compatibility mode for handling strings/bytes with the old specification + compatibility = False + + # Auto-detect system float precision + if sys.float_info.mant_dig == 53: + _float_precision = "double" + else: + _float_precision = "single" + + # Map packb and unpackb to the appropriate version + if sys.version_info[0] == 3: + pack = _pack3 + packb = _packb3 + dump = _pack3 + dumps = _packb3 + unpack = _unpack3 + unpackb = _unpackb3 + load = _unpack3 + loads = _unpackb3 + xrange = range + else: + pack = _pack2 + packb = _packb2 + dump = _pack2 + dumps = _packb2 + unpack = _unpack2 + unpackb = _unpackb2 + load = _unpack2 + loads = _unpackb2 + + # Build a dispatch table for fast lookup of unpacking function + + _unpack_dispatch_table = {} + # Fix uint + for code in range(0, 0x7f + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_integer + # Fix map + for code in range(0x80, 0x8f + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_map + # Fix array + for code in range(0x90, 0x9f + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_array + # Fix str + for code in range(0xa0, 0xbf + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_string + # Nil + _unpack_dispatch_table[b'\xc0'] = _unpack_nil + # Reserved + _unpack_dispatch_table[b'\xc1'] = _unpack_reserved + # Boolean + _unpack_dispatch_table[b'\xc2'] = _unpack_boolean + _unpack_dispatch_table[b'\xc3'] = _unpack_boolean + # Bin + for code in range(0xc4, 0xc6 + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_binary + # Ext + for code in range(0xc7, 0xc9 + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_ext + # Float + _unpack_dispatch_table[b'\xca'] = _unpack_float + _unpack_dispatch_table[b'\xcb'] = _unpack_float + # Uint + for code in range(0xcc, 0xcf + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_integer + # Int + for code in range(0xd0, 0xd3 + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_integer + # Fixext + for code in range(0xd4, 0xd8 + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_ext + # String + for code in range(0xd9, 0xdb + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_string + # Array + _unpack_dispatch_table[b'\xdc'] = _unpack_array + _unpack_dispatch_table[b'\xdd'] = _unpack_array + # Map + _unpack_dispatch_table[b'\xde'] = _unpack_map + _unpack_dispatch_table[b'\xdf'] = _unpack_map + # Negative fixint + for code in range(0xe0, 0xff + 1): + _unpack_dispatch_table[struct.pack("B", code)] = _unpack_integer + + +__init() diff --git a/src/python/director/treeviewer.py b/src/python/director/treeviewer.py index 31ba5aca4..e14e0506b 100644 --- a/src/python/director/treeviewer.py +++ b/src/python/director/treeviewer.py @@ -1,6 +1,7 @@ import json import math import os +import Queue import re import time import warnings @@ -9,7 +10,6 @@ from director import objectmodel as om from director import applogic as app -from director import lcmUtils from director import transformUtils from director.debugVis import DebugData from director import ioUtils @@ -19,9 +19,24 @@ from director import vtkNumpy as vnp from director import visualization as vis from director import packagepath -from director.shallowCopy import shallowCopy +from director import taskrunner +from director.timercallback import TimerCallback + +from director.thirdparty import umsgpack, msgpack_numpy + +try: + import zmq + HAVE_ZMQ = True +except ImportError: + HAVE_ZMQ = False + +try: + import robotlocomotion as lcmrl + from director import lcmUtils + HAVE_LCMRL = True +except ImportError: + HAVE_LCMRL = False -import robotlocomotion as lcmrl from PythonQt import QtGui @@ -39,7 +54,7 @@ class ViewerStatus: class ViewerResponse(namedtuple("ViewerResponse", ["status", "data"])): - def toJson(self): + def toDict(self): return dict(status=self.status, **self.data) @@ -403,101 +418,14 @@ def findPathToAncestor(fromItem, toItem): return path + class TreeViewer(object): name = "Remote Tree Viewer" - def __init__(self, view): - - self.subscriber = None + def __init__(self, view, zmqUrl=None, useLcm=True): self.view = view self.itemToPathCache = {} self.pathToItemCache = {} - self.client_id_regex = re.compile(r'\<(.*)\>') - self.enable() - self.sendStatusMessage( - 0, ViewerResponse(ViewerStatus.OK, {"ready": True})) - - def _addSubscriber(self): - self.subscriber = lcmUtils.addSubscriber( - 'DIRECTOR_TREE_VIEWER_REQUEST.*', - lcmrl.viewer2_comms_t, - self.onViewerRequest, - callbackNeedsChannel=True) - # Note: from discussion with @patmarion, there's a bug in the lcmUtils subscriber - # when dealing with regex channels: - # > If you subscribe to MY_CHANNEL_*, and two messages arrive back to back - # > (MY_CHANNEL_FOO, foo_data) and (MY_CHANNEL_BAR, bar_data), then the default - # > behavior in the lcm subscriber is to only call your callback once (if notify - # > all messages = false), and it calls callback(MY_CHANNEL_FOO, bar_data) - # - # However, this can be avoided by notifying for *all* messages, which is what we - # want anyway: - self.subscriber.setNotifyAllMessagesEnabled(True) - - def _removeSubscriber(self): - lcmUtils.removeSubscriber(self.subscriber) - self.subscriber = None - - def isEnabled(self): - return self.subscriber is not None - - def setEnabled(self, enabled): - if enabled and not self.isEnabled(): - self._addSubscriber() - elif not enabled and self.isEnabled(): - self._removeSubscriber() - - def enable(self): - self.setEnabled(True) - - def disable(self): - self.setEnabled(False) - - def sendStatusMessage(self, timestamp, response, client_id=""): - msg = lcmrl.viewer2_comms_t() - msg.format = "treeviewer_json" - msg.format_version_major = 1 - msg.format_version_minor = 0 - data = dict(timestamp=timestamp, **response.toJson()) - msg.data = bytearray(json.dumps(data), encoding='utf-8') - msg.num_bytes = len(msg.data) - if client_id: - channel = "DIRECTOR_TREE_VIEWER_RESPONSE_<{:s}>".format(client_id) - else: - channel = "DIRECTOR_TREE_VIEWER_RESPONSE" - lcmUtils.publish(channel, msg) - - def decodeCommsMsg(self, msg): - if msg.format == "treeviewer_json": - if msg.format_version_major == 1 and msg.format_version_minor == 0: - data = json.loads(msg.data.decode()) - return data, ViewerResponse(ViewerStatus.OK, {}) - else: - return None, ViewerResponse(ViewerStatus.ERROR_UNKNOWN_FORMAT_VERSION, - {"supported_formats": { - "treeviewer_json": ["1.0"] - }}) - else: - return None, ViewerResponse(ViewerStatus.ERROR_UNKNOWN_FORMAT, - {"supported_formats": { - "treeviewer_json": ["1.0"] - }}) - - def onViewerRequest(self, msg, channel="DIRECTOR_TREE_VIEWER_REQUEST"): - match = self.client_id_regex.search(channel) - if match: - client_id = match.group(1) # MatchObject.group is 1-indexed - else: - warnings.warn("To reduce cross-talk, clients should append a unique client ID inside <> characters to their DIRECTOR_TREE_VIEWER_REQUEST channel name. For example: DIRECTOR_TREE_VIEWER_REQUEST_. The client should also subscribe to the equivalent response channel: DIRECTOR_TREE_VIEWER_RESPONSE_") - client_id = "" - data, response = self.decodeCommsMsg(msg) - if data is None: - self.sendStatusMessage(msg.utime, - response, client_id) - else: - response = self.handleViewerRequest(data) - self.sendStatusMessage(msg.utime, - response, client_id) def handleViewerRequest(self, data): deletedPaths = set() @@ -631,3 +559,137 @@ def getPathFolder(self, path): self.pathToItemCache[path] = folder self.itemToPathCache[folder] = path return folder + + +class ZMQTreeViewer(TreeViewer): + name = "Remote Tree Viewer (ZeroMQ + MsgPack)" + + def __init__(self, view, zmqUrl): + super(ZMQTreeViewer, self).__init__(view) + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REP) + self.socket.bind(zmqUrl) + self.msgQueue = Queue.Queue() + self.taskRunner = taskrunner.TaskRunner() + self.taskRunner.callOnThread(self.listenZmq) + self.zmqTimer = TimerCallback(callback=self.handleZmq, targetFps=60) + self.zmqTimer.start() + + def isEnabled(self): + return self.taskRunner.timer.isActive() + + def setEnabled(self, enabled): + if enabled: + self.taskRunner.timer.start() + else: + self.taskRunner.timer.stop() + + def listenZmq(self): + while True: + message = self.socket.recv() + data = umsgpack.unpackb(message, + ext_handlers = { + 0x50: lambda ext: msgpack_numpy.decode(umsgpack.unpackb(ext.data)) + }) + self.socket.send("received") + self.msgQueue.put(data) + + def handleZmq(self): + while not self.msgQueue.empty(): + self.handleViewerRequest(self.msgQueue.get()) + + +class LCMTreeViewer(TreeViewer): + name = "Remote Tree Viewer (LCM)" + + def __init__(self, view): + super(LCMTreeViewer, self).__init__(view) + self.subscriber = None + self.client_id_regex = re.compile(r'\<(.*)\>') + # warnings.warn("The TreeViewer LCM protocol is deprecated. Please switch to the new ZeroMQ/MsgPack protocol, which should offer much better performance and reliability.") + self.enable() + self.sendStatusMessage( + 0, ViewerResponse(ViewerStatus.OK, {"ready": True})) + + def _addSubscriber(self): + self.subscriber = lcmUtils.addSubscriber( + 'DIRECTOR_TREE_VIEWER_REQUEST.*', + lcmrl.viewer2_comms_t, + self.onViewerRequest, + callbackNeedsChannel=True) + # Note: from discussion with @patmarion, there's a bug in the lcmUtils subscriber + # when dealing with regex channels: + # > If you subscribe to MY_CHANNEL_*, and two messages arrive back to back + # > (MY_CHANNEL_FOO, foo_data) and (MY_CHANNEL_BAR, bar_data), then the default + # > behavior in the lcm subscriber is to only call your callback once (if notify + # > all messages = false), and it calls callback(MY_CHANNEL_FOO, bar_data) + # + # However, this can be avoided by notifying for *all* messages, which is what we + # want anyway: + self.subscriber.setNotifyAllMessagesEnabled(True) + + def _removeSubscriber(self): + lcmUtils.removeSubscriber(self.subscriber) + self.subscriber = None + + def isEnabled(self): + return self.subscriber is not None + + def setEnabled(self, enabled): + if enabled and not self.isEnabled(): + self._addSubscriber() + elif not enabled and self.isEnabled(): + self._removeSubscriber() + + def enable(self): + self.setEnabled(True) + + def disable(self): + self.setEnabled(False) + + def sendStatusMessage(self, timestamp, response, client_id=""): + msg = lcmrl.viewer2_comms_t() + msg.format = "treeviewer_json" + msg.format_version_major = 1 + msg.format_version_minor = 0 + data = dict(timestamp=timestamp, **response.toDict()) + msg.data = bytearray(json.dumps(data), encoding='utf-8') + msg.num_bytes = len(msg.data) + if client_id: + channel = "DIRECTOR_TREE_VIEWER_RESPONSE_<{:s}>".format(client_id) + else: + channel = "DIRECTOR_TREE_VIEWER_RESPONSE" + lcmUtils.publish(channel, msg) + + def decodeCommsMsg(self, msg): + if msg.format == "treeviewer_json": + if msg.format_version_major == 1 and msg.format_version_minor == 0: + data = json.loads(msg.data.decode()) + return data, ViewerResponse(ViewerStatus.OK, {}) + else: + return None, ViewerResponse(ViewerStatus.ERROR_UNKNOWN_FORMAT_VERSION, + {"supported_formats": { + "treeviewer_json": ["1.0"] + }}) + else: + return None, ViewerResponse(ViewerStatus.ERROR_UNKNOWN_FORMAT, + {"supported_formats": { + "treeviewer_json": ["1.0"] + }}) + + def onViewerRequest(self, msg, channel="DIRECTOR_TREE_VIEWER_REQUEST"): + match = self.client_id_regex.search(channel) + if match: + client_id = match.group(1) # MatchObject.group is 1-indexed + else: + warnings.warn("To reduce cross-talk, clients should append a unique client ID inside <> characters to their DIRECTOR_TREE_VIEWER_REQUEST channel name. For example: DIRECTOR_TREE_VIEWER_REQUEST_. The client should also subscribe to the equivalent response channel: DIRECTOR_TREE_VIEWER_RESPONSE_") + client_id = "" + data, response = self.decodeCommsMsg(msg) + if data is None: + self.sendStatusMessage(msg.utime, + response, client_id) + else: + response = self.handleViewerRequest(data) + self.sendStatusMessage(msg.utime, + response, client_id) + diff --git a/src/python/tests/CMakeLists.txt b/src/python/tests/CMakeLists.txt index 3dc45e5fa..0d3a75cdd 100644 --- a/src/python/tests/CMakeLists.txt +++ b/src/python/tests/CMakeLists.txt @@ -63,7 +63,8 @@ set(python_tests_standalone # todo # need this special case until robotlocomotion/lcmtypes are added to openhumanoids if(NOT USE_DRC) - list(APPEND python_tests_lcm testTreeViewerInterface.py) + list(APPEND python_tests_lcm testTreeViewerLCMInterface.py) + list(APPEND python_tests_lcm testTreeViewerZMQInterface.py) list(APPEND python_tests_lcm testTreeViewerClient.py) list(APPEND python_tests_lcm testTreeViewerPolyLine.py) endif() diff --git a/src/python/tests/testTreeViewerInterface.py b/src/python/tests/testTreeViewerLCMInterface.py similarity index 100% rename from src/python/tests/testTreeViewerInterface.py rename to src/python/tests/testTreeViewerLCMInterface.py diff --git a/src/python/tests/testTreeViewerZMQInterface.py b/src/python/tests/testTreeViewerZMQInterface.py new file mode 100644 index 000000000..bfbf0c693 --- /dev/null +++ b/src/python/tests/testTreeViewerZMQInterface.py @@ -0,0 +1,38 @@ +import os +import subprocess + +import zmq + +from director.thirdparty import umsgpack + + +if __name__ == '__main__': + vis_binary = os.path.join(os.path.dirname(sys.executable), + "drake-visualizer") + vis_process = subprocess.Popen([vis_binary, '--testing', '--interactive', '--treeviewer-zmq-url=tcp://127.0.0.1:56300']) + context = zmq.Context() + socket = context.socket(zmq.REQ) + socket.connect("tcp://127.0.0.1:56300") + print("connected") + + data = { + "timestamp": 1486691399249288, + "setgeometry": [ + { + "path": ["robot1", "link1"], + "geometry": { + "type": "box", + "color": [1, 0, 0, 0.5], + "lengths": [1, 0.5, 2] + } + } + ], + "settransform": [], + "delete": [] + } + print("sending") + socket.send(umsgpack.packb(data)) + print("waiting for reply") + print(socket.recv()) + + vis_process.terminate()