├── .gitignore
├── src
    └── rqt_py_trees
    │   ├── plugins
    │       ├── __init__.py
    │       ├── plugin.py
    │       ├── message_view.py
    │       ├── timeline_renderer.py
    │       ├── topic_message_view.py
    │       └── raw_view.py
    │   ├── Readme.md
    │   ├── qt_dotgraph
    │       ├── __init__.py
    │       ├── graph_item.py
    │       ├── pygraphvizfactory.py
    │       ├── pydotfactory.py
    │       ├── edge_item.py
    │       ├── node_item.py
    │       └── dot_to_qt.py
    │   ├── __init__.py
    │   ├── timeline_listener.py
    │   ├── dynamic_timeline_listener.py
    │   ├── topic_helper.py
    │   ├── visibility.py
    │   ├── message_loader_thread.py
    │   ├── dotcode_behaviour.py
    │   ├── dynamic_timeline.py
    │   └── behaviour_tree.py
├── launch
    └── demo.launch
├── setup.py
├── .project
├── .pydevproject
├── CMakeLists.txt
├── plugin.xml
├── CHANGELOG.rst
├── scripts
    └── rqt_py_trees
├── LICENSE
├── package.xml
├── README.md
├── test
    └── dotcode_tf_test.py
└── resource
    └── RosBehaviourTree.ui


/.gitignore:
--------------------------------------------------------------------------------
1 | *.pyc
2 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/__init__.py:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/launch/demo.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <node name="rqt_py_trees" type="rqt_py_trees" pkg="rqt_py_trees"/>
3 |   <include file="$(find py_trees)/launch/demo_tree.launch"/>
4 | </launch>
5 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/Readme.md:
--------------------------------------------------------------------------------
1 | # About
2 | 
3 | This readme is about making sense of what is happening inside the rqt py trees program. I forget
4 | inbetween bouts of hacking on it.
5 | 
6 | 


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from distutils.core import setup
 4 | from catkin_pkg.python_setup import generate_distutils_setup
 5 | 
 6 | d = generate_distutils_setup(
 7 |     packages=['rqt_py_trees', 'rqt_py_trees.plugins', 'rqt_py_trees.qt_dotgraph'],
 8 |     package_dir={'': 'src'},
 9 |     scripts=['scripts/rqt_py_trees']
10 | )
11 | 
12 | setup(**d)
13 | 


--------------------------------------------------------------------------------
/.project:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <projectDescription>
 3 | 	<name>rqt_py_trees</name>
 4 | 	<comment></comment>
 5 | 	<projects>
 6 | 	</projects>
 7 | 	<buildSpec>
 8 | 		<buildCommand>
 9 | 			<name>org.python.pydev.PyDevBuilder</name>
10 | 			<arguments>
11 | 			</arguments>
12 | 		</buildCommand>
13 | 	</buildSpec>
14 | 	<natures>
15 | 		<nature>org.python.pydev.pythonNature</nature>
16 | 	</natures>
17 | </projectDescription>
18 | 


--------------------------------------------------------------------------------
/.pydevproject:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8" standalone="no"?>
2 | <?eclipse-pydev version="1.0"?><pydev_project>
3 | <pydev_pathproperty name="org.python.pydev.PROJECT_SOURCE_PATH">
4 | <path>/${PROJECT_DIR_NAME}/src</path>
5 | </pydev_pathproperty>
6 | <pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
7 | <pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">Default</pydev_property>
8 | </pydev_project>
9 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(rqt_py_trees)
 3 | set(CMAKE_CXX_FLAGS "-g ${CMAKE_CXX_FLAGS}")
 4 | # Load catkin and all dependencies required for this package
 5 | find_package(catkin REQUIRED py_trees_msgs rqt_bag)
 6 | catkin_package()
 7 | catkin_python_setup()
 8 | 
 9 | install(FILES plugin.xml
10 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
11 | )
12 | 
13 | install(DIRECTORY resource
14 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
15 | )
16 | 
17 | install(PROGRAMS scripts/rqt_py_trees
18 |   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
19 | )
20 | 


--------------------------------------------------------------------------------
/plugin.xml:
--------------------------------------------------------------------------------
 1 | <library path="src">
 2 |   <class name="RosBehaviourTree" type="rqt_py_trees.behaviour_tree.RosBehaviourTree" base_class_type="rqt_gui_py::Plugin">
 3 |     <description>
 4 |       A Python GUI plugin to visualize py_trees behaviour trees.
 5 |     </description>
 6 |     <qtgui>
 7 |       <group>
 8 |         <label>Visualization</label>
 9 |         <icon type="theme">folder</icon>
10 |         <statustip>Plugins related to visualization.</statustip>
11 |       </group>
12 |       <label>Behaviour Tree</label>
13 |       <icon type="theme">preferences-system-network</icon>
14 |       <statustip>A Python GUI plugin for visualizing py_trees behaviour trees.</statustip>
15 |     </qtgui>
16 |   </class>
17 | </library>
18 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | Changelog
 2 | =========
 3 | 
 4 | 0.4.1 (2021-05-09)
 5 | ------------------
 6 | * bugfix a python2->3 iterator->list generator in dynamic_timeline
 7 | * provide a --topic argument on the command line
 8 | 
 9 | 0.4.0 (2020-08-03)
10 | ------------------
11 | * python3 and noetic upgrades
12 | 
13 | 0.3.1 (2017-04-11)
14 | ------------------
15 | * add missing python-pygraphviz dependency
16 | 
17 | 0.3.0 (2017-03-02)
18 | ------------------
19 | * getting ready for first kinetic release
20 | 
21 | 0.2.0 (2017-01-13)
22 | ------------------
23 | * drop 'd' as an option since rqt stole it
24 | * parallels and choosers now draw notes and double octagons like dot graphs
25 | 
26 | 0.1.0 (2016-08-25)
27 | ------------------
28 | * initial import from Yujin
29 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/__init__.py:
--------------------------------------------------------------------------------
 1 | #
 2 | # License: BSD
 3 | #   https://raw.github.com/stonier/rqt_py_trees/license/LICENSE
 4 | #
 5 | ##############################################################################
 6 | # Documentation
 7 | ##############################################################################
 8 | 
 9 | """
10 | Repackages the qt_dotgraph_ ROS package. There are a few
11 | problems with both that and the underlying pydot/pygraphviz
12 | packages, so this has been brought in here for experimentation.
13 | 
14 | .. _py_trees: http://wiki.ros.org/qt_dotgraph
15 | 
16 | """
17 | ##############################################################################
18 | # Imports
19 | ##############################################################################
20 | 
21 | from . import dot_to_qt
22 | from . import edge_item, node_item, graph_item
23 | from . import pydotfactory


--------------------------------------------------------------------------------
/src/rqt_py_trees/__init__.py:
--------------------------------------------------------------------------------
 1 | #
 2 | # License: BSD
 3 | #   https://raw.github.com/stonier/rqt_py_trees/license/LICENSE
 4 | #
 5 | ##############################################################################
 6 | # Documentation
 7 | ##############################################################################
 8 | 
 9 | """
10 | The rqt behaviour tree plugin.
11 | """
12 | ##############################################################################
13 | # Imports
14 | ##############################################################################
15 | 
16 | from . import behaviour_tree
17 | from . import dotcode_behaviour
18 | from . import dynamic_timeline_frame
19 | from . import dynamic_timeline_listener
20 | from . import dynamic_timeline
21 | from . import message_loader_thread
22 | from . import timeline_listener
23 | from . import topic_helper
24 | from . import visibility
25 | 
26 | # subpackages
27 | from . import plugins
28 | from . import qt_dotgraph


--------------------------------------------------------------------------------
/scripts/rqt_py_trees:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | import sys
 4 | import signal
 5 | try:  # indigo
 6 |     from python_qt_binding.QtGui import QApplication, QMainWindow
 7 | except ImportError:  # kinetic+ (pyqt5)
 8 |     from python_qt_binding.QtWidgets import QApplication, QMainWindow
 9 | 
10 | from rqt_py_trees.behaviour_tree import RosBehaviourTree
11 | 
12 | from rqt_gui.main import Main
13 | 
14 | # Crude, but needed to make sure that the display of all the GUI arguments works
15 | # when the -h option is given. If we try to parse options before they won't be
16 | # displayed.
17 | if RosBehaviourTree.no_roscore_switch in sys.argv:
18 |     app = QApplication(sys.argv)
19 |     window = QMainWindow()
20 |     myapp = RosBehaviourTree(window)
21 |     window.show()
22 |     signal.signal(signal.SIGINT, signal.SIG_DFL)
23 |     sys.exit(app.exec_())
24 | else:
25 |     main = Main()
26 |     sys.exit(main.main(sys.argv, standalone='rqt_py_trees.behaviour_tree.RosBehaviourTree', plugin_argument_provider=RosBehaviourTree.add_arguments))
27 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/timeline_listener.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from rqt_bag import MessageView
 4 | 
 5 | class TimelineListener(MessageView):
 6 |     """Basic listener for the timeline. The current message can be accessed using
 7 |     the ``msg`` member.
 8 | 
 9 |     """
10 |     def __init__(self, timeline, topic, signal_viewed, signal_cleared):
11 |         """
12 |         :param timeline: the timeline that this object is attached to
13 |         :param topic: the topic that this object is interested in
14 |         :param signal_viewed: the signal that should be emitted when the message being viewed changes
15 |         :param signal_cleared: the signal that should be emitted when the message being viewed is cleared
16 |         """
17 |         super(TimelineListener, self).__init__(timeline, topic)
18 |         self.signal_viewed = signal_viewed
19 |         self.signal_cleared = signal_cleared
20 |         self.msg = None
21 | 
22 |     def message_viewed(self, bag, msg_details):
23 |         """Called whenever the message is updated. Updates the stored message and emits
24 |         a signal.
25 | 
26 |         """
27 |         self.msg = msg_details[1]
28 |         self.signal_viewed.emit()
29 |     
30 |     def message_cleared(self):
31 |         self.signal_cleared.emit()
32 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/dynamic_timeline_listener.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | import rospy
 4 | from .plugins.message_view import MessageView
 5 | 
 6 | class DynamicTimelineListener(MessageView):
 7 |     """Basic listener for the timeline. The current message can be accessed using
 8 |     the ``msg`` member.
 9 | 
10 |     """
11 |     def __init__(self, timeline, topic, signal_viewed, signal_cleared):
12 |         """
13 |         :param timeline: the timeline that this object is attached to
14 |         :param topic: the topic that this object is interested in
15 |         :param signal_viewed: the signal that should be emitted when the message being viewed changes
16 |         :param signal_cleared: the signal that should be emitted when the message being viewed is cleared
17 |         """
18 |         super(DynamicTimelineListener, self).__init__(timeline, topic)
19 |         self.signal_viewed = signal_viewed
20 |         self.signal_cleared = signal_cleared
21 |         self.msg = None
22 | 
23 |     def message_viewed(self, msg_details):
24 |         """Called whenever the message is updated. Updates the stored message and emits
25 |         a signal.
26 |         """
27 |         self.msg = msg_details
28 |         # rospy.loginfo("got message {0} on topic {1}".format(msg_details.behaviours[0].name, self.topic))
29 |         self.signal_viewed.emit()
30 | 
31 |     def message_cleared(self):
32 |         self.signal_cleared.emit()
33 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/graph_item.py:
--------------------------------------------------------------------------------
 1 | #
 2 | # License: Yujin
 3 | #
 4 | ##############################################################################
 5 | # Description
 6 | ##############################################################################
 7 | 
 8 | """
 9 | .. module:: graph_item
10 |    :platform: Unix
11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.graph_item module.
12 | 
13 | Oh my spaghettified magnificence,
14 | Bless my noggin with a tickle from your noodly appendages!
15 | 
16 | """
17 | 
18 | ##############################################################################
19 | # Imports
20 | ##############################################################################
21 | 
22 | from python_qt_binding.QtGui import QColor
23 | try:  # indigo
24 |     from python_qt_binding.QtGui import QGraphicsItemGroup
25 | except ImportError:  # kinetic+ (pyqt5)
26 |     from python_qt_binding.QtWidgets import QGraphicsItemGroup
27 | 
28 | ##############################################################################
29 | # Classes
30 | ##############################################################################
31 | 
32 | 
33 | class GraphItem(QGraphicsItemGroup):
34 | 
35 |     _COLOR_BLACK = QColor(0, 0, 0)
36 |     _COLOR_BLUE = QColor(0, 0, 204)
37 |     _COLOR_GREEN = QColor(0, 170, 0)
38 |     _COLOR_ORANGE = QColor(255, 165, 0)
39 |     _COLOR_RED = QColor(255, 0, 0)
40 |     _COLOR_TEAL = QColor(0, 170, 170)
41 | 
42 |     def __init__(self, highlight_level, parent=None):
43 |         super(GraphItem, self).__init__(parent)
44 |         self._highlight_level = highlight_level
45 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | # Software License Agreement (BSD License)
 2 | #
 3 | # Copyright (c) 2015 Daniel Stonier
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions
 8 | # are met:
 9 | #
10 | #    * Redistributions of source code must retain the above copyright
11 | #        notice, this list of conditions and the following disclaimer.
12 | #    * Redistributions in binary form must reproduce the above
13 | #        copyright notice, this list of conditions and the following
14 | #        disclaimer in the documentation and/or other materials provided
15 | #        with the distribution.
16 | #    * Neither the name of Yujin Robot nor the names of its
17 | #        contributors may be used to endorse or promote products derived
18 | #        from this software without specific prior written permission.
19 | #
20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 | 
33 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>rqt_py_trees</name>
 3 |   <version>0.4.1</version>
 4 |   <description>
 5 |     rqt_py_trees provides a GUI plugin for visualizing py_trees behaviour trees based on rqt_tf_tree.
 6 |   </description>
 7 | 
 8 |   <maintainer email="m.staniaszek@gmail.com">Michal Staniaszek</maintainer>
 9 |   <maintainer email="stonier@yujinrobot.com">Daniel Stonier</maintainer>
10 |   <maintainer email="naveedhd@gmail.com">Naveed Usmani</maintainer>
11 | 
12 |   <license>BSD</license>  
13 | 
14 |   <url type="website">http://ros.org/wiki/rqt_py_trees</url>
15 |   <url type="repository">https://github.com/stonier/rqt_py_trees</url>
16 |   <url type="bugtracker">https://github.com/stonier/rqt_py_trees/issues</url>
17 | 
18 |   <author>Thibault Kruse</author>
19 |   <author>Michal Staniaszek</author>
20 |   <author>Daniel Stonier</author>
21 |   <author>Naveed Usmani</author>
22 | 
23 |   <buildtool_depend>catkin</buildtool_depend>
24 |   <build_depend>py_trees</build_depend>
25 |   <build_depend>py_trees_msgs</build_depend>
26 |   <build_depend>rqt_bag</build_depend>
27 | 
28 |   <run_depend>geometry_msgs</run_depend>
29 |   <run_depend>python3-rospkg</run_depend>
30 |   <run_depend>python3-termcolor</run_depend>
31 |   <run_depend>qt_dotgraph</run_depend>
32 |   <run_depend>rospy</run_depend>
33 |   <run_depend>rqt_graph</run_depend>
34 |   <run_depend>rqt_gui</run_depend>
35 |   <run_depend>rqt_gui_py</run_depend>
36 |   <run_depend>py_trees</run_depend>
37 |   <run_depend>py_trees_msgs</run_depend>
38 |   <run_depend>rqt_bag</run_depend>
39 |   <run_depend>unique_id</run_depend>
40 |   <run_depend>python3-pygraphviz</run_depend>
41 | 
42 |   <test_depend>python3-mock</test_depend>
43 | 
44 |   <export>
45 |     <rqt_gui plugin="${prefix}/plugin.xml"/>
46 |   </export>
47 | </package>
48 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | *ROS1 only!!!*
 2 | 
 3 | *See [py_trees_ros_viewer](https://github.com/splintered-reality/py_trees_ros_viewer) for the ROS2 visualisation tool for py_trees.*
 4 | 
 5 | # Rqt Py Trees
 6 | 
 7 | Refer to the sphinx documentation for [py_trees](http://py-trees.readthedocs.io/en/devel/) as a primer and reference on behaviour trees & [py_trees_ros](https://stonier.github.io/py_trees_ros/) for tutorials on getting started with behaviour trees in ROS.
 8 | 
 9 | ## The ROS Py Trees Packages
10 | 
11 | * [py_trees](https://github.com/stonier/py_trees) ([sphinx documentation](http://py-trees.readthedocs.io/en/devel/))
12 | * [py_trees_msgs](https://github.com/stonier/py_trees_msgs)
13 | * [py_trees_ros](https://github.com/stonier/py_trees_ros) ([sphinx documentation](https://stonier.github.io/py_trees_ros/))
14 | * [rqt_py_trees](https://github.com/stonier/rqt_py_trees)
15 | 
16 | 
17 | ## Build Status
18 | 
19 | | Devel | Kinetic |
20 | |:---:|:---:|
21 | | [![devel-Sources][devel-sources-image]][devel-sources] | [![0.3.x-Sources][0.3.x-sources-image]][0.3.x-sources] |
22 | | [![devel-Status][devel-build-status-image]][devel-build-status] | [![kinetic-Status][kinetic-build-status-image]][kinetic-build-status] | |
23 | 
24 | [devel-sources-image]: http://img.shields.io/badge/sources-devel-blue.svg?style=plastic
25 | [devel-sources]: https://github.com/stonier/rqt_py_trees/tree/devel
26 | [0.3.x-sources-image]: http://img.shields.io/badge/sources-0.3.x--kinetic-blue.svg?style=plastic
27 | [0.3.x-sources]: https://github.com/stonier/rqt_py_trees/tree/release/0.3-kinetic
28 | 
29 | [devel-build-status-image]: http://build.ros.org/job/Kdev__rqt_py_trees__ubuntu_xenial_amd64/badge/icon?style=plastic
30 | [devel-build-status]: http://build.ros.org/job/Kdev__rqt_py_trees__ubuntu_xenial_amd64                     
31 | [kinetic-build-status-image]: http://build.ros.org/job/Kbin_uX64__rqt_py_trees__ubuntu_xenial_amd64__binary/badge/icon?style=plastic
32 | [kinetic-build-status]: http://build.ros.org/job/Kbin_uX64__rqt_py_trees__ubuntu_xenial_amd64__binary
33 | 
34 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/plugin.py:
--------------------------------------------------------------------------------
 1 | # Software License Agreement (BSD License)
 2 | #
 3 | # Copyright (c) 2012, Willow Garage, Inc.
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions
 8 | # are met:
 9 | #
10 | #  * Redistributions of source code must retain the above copyright
11 | #    notice, this list of conditions and the following disclaimer.
12 | #  * Redistributions in binary form must reproduce the above
13 | #    copyright notice, this list of conditions and the following
14 | #    disclaimer in the documentation and/or other materials provided
15 | #    with the distribution.
16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
17 | #    contributors may be used to endorse or promote products derived
18 | #    from this software without specific prior written permission.
19 | #
20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 | 
33 | 
34 | class Plugin(object):
35 | 
36 |     """
37 |     Interface for rqt_bag plugins.
38 |     User-defined plugins may either subclass `rqt_bag.plugin.Plugin` or according to duck typing implement only the needed methods.
39 |     """
40 | 
41 |     def __init__(self):
42 |         pass
43 | 
44 |     def get_view_class(self):
45 |         """Return a class which is a child of rqt_bag.plugin.topic_message_view.TopicMessageView."""
46 |         raise NotImplementedError()
47 | 
48 |     def get_renderer_class(self):
49 |         """
50 |         Return a class which is a child of rqt_bag.plugin.timeline_renderer.TimelineRenderer.
51 |         To omit the renderer component simply return None.
52 |         """
53 |         return None
54 | 
55 |     def get_message_types(self):
56 |         """
57 |         Return  alist of message types which this plugin operates on.
58 |         To allow your plugin to be run on all message types return ['*'].
59 |         """
60 |         return []
61 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/topic_helper.py:
--------------------------------------------------------------------------------
 1 | # Software License Agreement (BSD License)
 2 | #
 3 | # Copyright (c) 2012, Willow Garage, Inc.
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions
 8 | # are met:
 9 | #
10 | #  * Redistributions of source code must retain the above copyright
11 | #    notice, this list of conditions and the following disclaimer.
12 | #  * Redistributions in binary form must reproduce the above
13 | #    copyright notice, this list of conditions and the following
14 | #    disclaimer in the documentation and/or other materials provided
15 | #    with the distribution.
16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
17 | #    contributors may be used to endorse or promote products derived
18 | #    from this software without specific prior written permission.
19 | #
20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICTS
29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 | 
33 | """
34 | Helper functions for bag files and timestamps.
35 | """
36 | 
37 | import time
38 | import rospy
39 | 
40 | 
41 | def stamp_to_str(t):
42 |     """
43 |     Convert a rospy.Time to a human-readable string.
44 | 
45 |     @param t: time to convert
46 |     @type  t: rospy.Time
47 |     """
48 |     t_sec = t.to_sec()
49 |     if t < rospy.Time.from_sec(60 * 60 * 24 * 365 * 5):
50 |         # Display timestamps earlier than 1975 as seconds
51 |         return '%.3fs' % t_sec
52 |     else:
53 |         return time.strftime('%b %d %Y %H:%M:%S', time.localtime(t_sec)) + '.%03d' % (t.nsecs / 1000000)
54 | 
55 | def get_start_stamp(topic):
56 |     """
57 |     Get the earliest timestamp in the topic.
58 | 
59 |     :param topic: topic tuple
60 |     :type topic: ``DynamicTimeline.Topic`` named tuple
61 |     :return: earliest timestamp, ``rospy.Time``
62 |     """
63 |     start_stamp = None
64 |     try:
65 |         start_stamp = topic.queue[0].stamp
66 |     except IndexError:
67 |         pass
68 | 
69 |     return start_stamp
70 | 
71 | def get_end_stamp(topic):
72 |     """
73 |     Get the latest timestamp in the topic.
74 | 
75 |     :param topic: topic tuple
76 |     :type topic: ``DynamicTimeline.Topic`` named tuple
77 |     :return: latest timestamp, ``rospy.Time``
78 |     """
79 |     end_stamp = None
80 |     try:
81 |         end_stamp = topic.queue[-1].stamp
82 |     except IndexError:
83 |         pass
84 | 
85 |     return end_stamp
86 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/message_view.py:
--------------------------------------------------------------------------------
 1 | # Software License Agreement (BSD License)
 2 | #
 3 | # Copyright (c) 2012, Willow Garage, Inc.
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions
 8 | # are met:
 9 | #
10 | #  * Redistributions of source code must retain the above copyright
11 | #    notice, this list of conditions and the following disclaimer.
12 | #  * Redistributions in binary form must reproduce the above
13 | #    copyright notice, this list of conditions and the following
14 | #    disclaimer in the documentation and/or other materials provided
15 | #    with the distribution.
16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
17 | #    contributors may be used to endorse or promote products derived
18 | #    from this software without specific prior written permission.
19 | #
20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 | 
33 | 
34 | from python_qt_binding.QtCore import QObject
35 | 
36 | 
37 | class MessageView(QObject):
38 |     """
39 |     A message details renderer. When registered with rqt_bag, a MessageView is called
40 |     whenever the timeline playhead moves.
41 |     """
42 |     name = 'Untitled'
43 | 
44 |     def __init__(self, timeline, topic):
45 |         super(MessageView, self).__init__()
46 |         self.timeline = timeline
47 |         self.topic = topic
48 | 
49 |     def message_viewed(self, msg_details):
50 |         """View the message.
51 | 
52 |         :param msg_details: the details of the message to be viewed
53 |         :type msg_details: Message namedtuple (stamp, message). Stamp is the
54 |             timestamp, message is the message received
55 | 
56 |         """
57 |         pass
58 | 
59 |     def message_cleared(self):
60 |         """
61 |         Clear the currently viewed message (if any).
62 |         """
63 |         pass
64 | 
65 |     def timeline_changed(self):
66 |         """
67 |         Called when the messages in a timeline change, e.g. if a new message is recorded, or
68 |         a bag file is added
69 |         """
70 |         pass
71 | 
72 |     def close(self):
73 |         """
74 |         Close the message view, releasing any resources.
75 |         """
76 |         pass
77 | 
78 | # NOTE: event function should not be changed in subclasses
79 |     def event(self, event):
80 |         """
81 |         This function will be called to process events posted by post_event
82 |         it will call message_cleared or message_viewed with the relevant data
83 |         """
84 |         msg_data = event.data
85 |         if msg_data:
86 |             self.message_viewed(msg_data)
87 |         else:
88 |             self.message_cleared()
89 |         return True
90 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/timeline_renderer.py:
--------------------------------------------------------------------------------
 1 | # Software License Agreement (BSD License)
 2 | #
 3 | # Copyright (c) 2012, Willow Garage, Inc.
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions
 8 | # are met:
 9 | #
10 | #  * Redistributions of source code must retain the above copyright
11 | #    notice, this list of conditions and the following disclaimer.
12 | #  * Redistributions in binary form must reproduce the above
13 | #    copyright notice, this list of conditions and the following
14 | #    disclaimer in the documentation and/or other materials provided
15 | #    with the distribution.
16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
17 | #    contributors may be used to endorse or promote products derived
18 | #    from this software without specific prior written permission.
19 | #
20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 | 
33 | from python_qt_binding.QtCore import QObject
34 | 
35 | 
36 | class TimelineRenderer(QObject):
37 |     """
38 |     A custom renderer for interval of time of a topic on the timeline.
39 | 
40 |     @param msg_combine_px: don't draw discrete messages if they're less than this many pixels separated [default: 1.5]
41 |     @type  msg_combine_px: float
42 |     """
43 |     def __init__(self, timeline, msg_combine_px=1.5):
44 |         self.timeline = timeline
45 |         self.msg_combine_px = msg_combine_px
46 | 
47 |     def get_segment_height(self, topic):
48 |         """
49 |         Get the height of the topic segment on the timeline.
50 | 
51 |         @param topic: topic name to draw
52 |         @type  topic: str
53 |         @return: height in pixels of the topic segment. If none, the timeline default is used.
54 |         @rtype:  int or None
55 |         """
56 |         return None
57 | 
58 |     def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height):
59 |         """
60 |         Draw the timeline segment.
61 | 
62 |         @param painter: QPainter context to render into
63 |         @param topic: topic name
64 |         @param stamp_start: start of the interval on the timeline
65 |         @param stamp_end: start of the interval on the timeline
66 |         @param x: x coordinate of the timeline interval
67 |         @param y: y coordinate of the timeline interval
68 |         @param width: width in pixels of the timeline interval
69 |         @param height: height in pixels of the timeline interval
70 |         @return: whether the interval was renderered
71 |         @rtype:  bool
72 |         """
73 |         return False
74 | 
75 |     def close(self):
76 |         """
77 |         Close the renderer, releasing any resources.
78 |         """
79 |         pass
80 | 


--------------------------------------------------------------------------------
/test/dotcode_tf_test.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | # Software License Agreement (BSD License)
 3 | #
 4 | # Copyright (c) 2009, Willow Garage, Inc.
 5 | # All rights reserved.
 6 | #
 7 | # Redistribution and use in source and binary forms, with or without
 8 | # modification, are permitted provided that the following conditions
 9 | # are met:
10 | #
11 | #  * Redistributions of source code must retain the above copyright
12 | #    notice, this list of conditions and the following disclaimer.
13 | #  * Redistributions in binary form must reproduce the above
14 | #    copyright notice, this list of conditions and the following
15 | #    disclaimer in the documentation and/or other materials provided
16 | #    with the distribution.
17 | #  * Neither the name of Willow Garage, Inc. nor the names of its
18 | #    contributors may be used to endorse or promote products derived
19 | #    from this software without specific prior written permission.
20 | #
21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | # POSSIBILITY OF SUCH DAMAGE.
33 | 
34 | # import unittest
35 | # import rospkg
36 | 
37 | # import tf
38 | # from tf.srv import *
39 | 
40 | # # get mock from pypi as 'mock'
41 | # from mock import Mock, MagicMock, patch
42 | 
43 | # from rqt_tf_tree.dotcode_tf import RosTfTreeDotcodeGenerator
44 | 
45 | 
46 | # class DotcodeGeneratorTest(unittest.TestCase):
47 | 
48 | #     def test_generate_dotcode(self):
49 | #         with patch('tf.TransformListener') as tf:
50 | #             def tf_srv_fun_mock():
51 | #                 return tf
52 | 
53 | #             yaml_data = {'frame1': {'parent': 'fr_parent',
54 | #                                             'broadcaster': 'fr_broadcaster',
55 | #                                             'rate': 'fr_rate',
56 | #                                             'buffer_length': 'fr_buffer_length',
57 | #                                             'most_recent_transform': 'fr_most_recent_transform',
58 | #                                             'oldest_transform': 'fr_oldest_transform',}}
59 | #             tf.frame_yaml = str(yaml_data)
60 |             
61 | #             factoryMock = Mock()
62 | #             graphMock = Mock()
63 | #             timeMock = Mock()
64 | #             timerMock = Mock()
65 | #             timerMock.now.return_value=timeMock
66 | #             timeMock.to_sec.return_value=42
67 | 
68 | #             yamlmock = Mock()
69 | #             yamlmock.load.return_value = yaml_data
70 |             
71 | #             factoryMock.create_dot.return_value = "foo"
72 | #             factoryMock.get_graph.return_value = graphMock
73 |             
74 | #             gen = RosTfTreeDotcodeGenerator(0)
75 | #             graph = gen.generate_dotcode(factoryMock, tf_srv_fun_mock, timerMock)
76 | 
77 | #             timerMock.now.assert_called_with()
78 | #             timeMock.to_sec.assert_called_with()
79 | #             factoryMock.create_dot.assert_called_with(graphMock)
80 |             
81 | #             self.assertEqual(graph, 'foo')
82 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/pygraphvizfactory.py:
--------------------------------------------------------------------------------
  1 | #
  2 | # License: Yujin
  3 | #
  4 | ##############################################################################
  5 | # Description
  6 | ##############################################################################
  7 | 
  8 | """
  9 | .. module:: pygraphvizfactory
 10 |    :platform: Unix
 11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.pygraphvizfactory module.
 12 | 
 13 | Oh my spaghettified magnificence,
 14 | Bless my noggin with a tickle from your noodly appendages!
 15 | 
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | import pygraphviz
 23 | 
 24 | ##############################################################################
 25 | # Classes
 26 | ##############################################################################
 27 | 
 28 | 
 29 | # Reference implementation for a dotcode factory
 30 | class PygraphvizFactory():
 31 | 
 32 |     def __init__(self):
 33 |         pass
 34 | 
 35 |     def get_graph(self, graph_type='digraph', rank='same', simplify=True, rankdir='TB', ranksep=0.2, compound=True):
 36 |         graph = pygraphviz.AGraph(directed=(graph_type == 'digraph'), ranksep=ranksep, rankdir=rankdir, rank=rank, compound=True, simplify=simplify)
 37 |         return graph
 38 | 
 39 |     def add_node_to_graph(self,
 40 |                           graph,
 41 |                           nodename,
 42 |                           nodelabel=None,
 43 |                           shape='box',
 44 |                           color=None,
 45 |                           url=None,
 46 |                           tooltip=None):
 47 |         """
 48 |         creates a node item for this factory, adds it to the graph.
 49 |         Node name can vary from label but must always be same for the same node label
 50 |         """
 51 |         if nodename is None or nodename == '':
 52 |             raise ValueError('Empty Node name')
 53 |         if nodelabel is None:
 54 |             nodelabel = nodename
 55 | 
 56 |         kwargs = {}
 57 |         if tooltip is not None:
 58 |             kwargs['tooltip'] = tooltip
 59 |         if color is not None:
 60 |             kwargs['color'] = color
 61 | 
 62 |         graph.add_node(nodename, label=str(nodelabel), shape=shape, url=url, **kwargs)
 63 | 
 64 |     def add_subgraph_to_graph(self,
 65 |                               graph,
 66 |                               subgraphlabel,
 67 |                               rank='same',
 68 |                               simplify=True,
 69 |                               rankdir='TB',
 70 |                               ranksep=0.2,
 71 |                               compound=True,
 72 |                               color=None,
 73 |                               shape='box',
 74 |                               style='bold'):
 75 |         """
 76 |         creates a cluster subgraph  item for this factory, adds it to the graph.
 77 |         cluster name can vary from label but must always be same for the same node label.
 78 |         Most layouters require cluster names to start with cluster.
 79 |         """
 80 |         if subgraphlabel is None or subgraphlabel == '':
 81 |             raise ValueError('Empty subgraph label')
 82 | 
 83 |         sg = graph.add_subgraph(name="cluster_%s" % subgraphlabel, ranksep=ranksep, rankdir=rankdir, rank=rank, compound=compound, label=str(subgraphlabel), style=style, color=color)
 84 | 
 85 |         return sg
 86 | 
 87 |     def add_edge_to_graph(self, graph, nodename1, nodename2, label=None, url=None, simplify=True, style=None, penwidth=1, color=None):
 88 |         kwargs = {'url': url}
 89 |         if label is not None:
 90 |             kwargs['label'] = label
 91 |         if style is not None:
 92 |             kwargs['style'] = style
 93 |         kwargs['penwidth'] = str(penwidth)
 94 |         if color is not None:
 95 |             kwargs['colorR'] = str(color[0])
 96 |             kwargs['colorG'] = str(color[1])
 97 |             kwargs['colorB'] = str(color[2])
 98 |         graph.add_edge(nodename1, nodename2, **kwargs)
 99 | 
100 |     def create_dot(self, graph):
101 |         graph.layout('dot')
102 |         # sadly pygraphviz generates line wraps cutting between numbers
103 |         return graph.string().replace("\\\n", "")
104 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/visibility.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | #
  3 | # License: BSD
  4 | #   https://raw.github.com/stonier/rqt_py_trees/license/LICENSE
  5 | #
  6 | ##############################################################################
  7 | # Documentation
  8 | ##############################################################################
  9 | 
 10 | """
 11 | .. module:: visibility
 12 |    :platform: Unix
 13 |    :synopsis: Converting back and forth the visibility level formats.
 14 | 
 15 | We have a python variable, a ros msg variable, and also the combobox variable!
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | import collections
 23 | import py_trees
 24 | import py_trees_msgs.msg as py_trees_msgs
 25 | import uuid_msgs.msg as uuid_msgs
 26 | import unique_id
 27 | 
 28 | ##############################################################################
 29 | # Imports
 30 | ##############################################################################
 31 | 
 32 | combo_to_py_trees = collections.OrderedDict([
 33 |     ("All", py_trees.common.VisibilityLevel.ALL),
 34 |     ("Detail", py_trees.common.VisibilityLevel.DETAIL),
 35 |     ("Component", py_trees.common.VisibilityLevel.COMPONENT),
 36 |     ("Big Picture", py_trees.common.VisibilityLevel.BIG_PICTURE)]
 37 | )
 38 | 
 39 | saved_setting_to_combo_index = {
 40 |     0: 0,
 41 |     1: 1,
 42 |     2: 2,
 43 |     3: 3
 44 | }
 45 | 
 46 | msg_to_py_trees = {
 47 |     py_trees_msgs.Behaviour.BLACKBOX_LEVEL_DETAIL: py_trees.common.BlackBoxLevel.DETAIL,
 48 |     py_trees_msgs.Behaviour.BLACKBOX_LEVEL_COMPONENT: py_trees.common.BlackBoxLevel.COMPONENT,
 49 |     py_trees_msgs.Behaviour.BLACKBOX_LEVEL_BIG_PICTURE: py_trees.common.BlackBoxLevel.BIG_PICTURE,
 50 |     py_trees_msgs.Behaviour.BLACKBOX_LEVEL_NOT_A_BLACKBOX: py_trees.common.BlackBoxLevel.NOT_A_BLACKBOX
 51 | }
 52 | 
 53 | 
 54 | def is_root(behaviour_id):
 55 |     """
 56 |     Check the unique id to determine if it is the root (all zeros).
 57 | 
 58 |     :param uuid.UUID behaviour_id:
 59 |     """
 60 |     return behaviour_id == unique_id.fromMsg(uuid_msgs.UniqueID())
 61 | 
 62 | 
 63 | def get_branch_blackbox_level(behaviours, behaviour_id, current_level):
 64 |     """
 65 |     Computes the critial (minimum) blackbox level present in the branch above
 66 |     this behaviour.
 67 | 
 68 |     :param {id: py_trees_msgs.Behaviour} behaviours: (sub)tree of all behaviours, including this one
 69 |     :param uuid.UUID behaviour_id: id of this behavour
 70 |     :param py_trees.common.BlackBoxLevel current_level
 71 |     """
 72 |     if is_root(behaviour_id):
 73 |         return current_level
 74 |     parent_id = unique_id.fromMsg(behaviours[behaviour_id].parent_id)
 75 |     new_level = min(behaviours[behaviour_id].blackbox_level, current_level)
 76 |     return get_branch_blackbox_level(behaviours, parent_id, new_level)
 77 | 
 78 | 
 79 | def is_visible(behaviours, behaviour_id, visibility_level):
 80 |     """
 81 |     :param {id: py_trees_msgs.Behaviour} behaviours:
 82 |     :param uuid.UUID behaviour_id:
 83 |     :param py_trees.common.VisibilityLevel visibility_level
 84 |     """
 85 |     branch_blackbox_level = get_branch_blackbox_level(
 86 |         behaviours,
 87 |         unique_id.fromMsg(behaviours[behaviour_id].parent_id),
 88 |         py_trees.common.BlackBoxLevel.NOT_A_BLACKBOX
 89 |     )
 90 |     # see also py_trees.display.generate_pydot_graph
 91 |     return visibility_level < branch_blackbox_level
 92 | 
 93 | 
 94 | def filter_behaviours_by_visibility_level(behaviours, visibility_level):
 95 |     """
 96 |     Drops any behaviours whose blackbox level does not match the required visibility
 97 |     level. See the py_trees.common module for more information.
 98 | 
 99 |     :param py_trees_msgs.msg.Behaviour[] behaviours
100 |     :returns: py_trees_msgs.msg.Behaviour[]
101 |     """
102 |     behaviours_by_id = {unique_id.fromMsg(b.own_id): b for b in behaviours}
103 |     visible_behaviours = [b for b in behaviours if is_visible(behaviours_by_id,
104 |                                                               unique_id.fromMsg(b.own_id),
105 |                                                               visibility_level)
106 |                           ]
107 |     return visible_behaviours
108 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/message_loader_thread.py:
--------------------------------------------------------------------------------
  1 | # Software License Agreement (BSD License)
  2 | #
  3 | # Copyright (c) 2012, Willow Garage, Inc.
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions
  8 | # are met:
  9 | #
 10 | #  * Redistributions of source code must retain the above copyright
 11 | #    notice, this list of conditions and the following disclaimer.
 12 | #  * Redistributions in binary form must reproduce the above
 13 | #    copyright notice, this list of conditions and the following
 14 | #    disclaimer in the documentation and/or other materials provided
 15 | #    with the distribution.
 16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
 17 | #    contributors may be used to endorse or promote products derived
 18 | #    from this software without specific prior written permission.
 19 | #
 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 31 | # POSSIBILITY OF SUCH DAMAGE.
 32 | 
 33 | import threading
 34 | 
 35 | 
 36 | class MessageLoaderThread(threading.Thread):
 37 |     """
 38 |     Waits for a new playhead position on the given topic, then loads the message at that position and notifies the view threads.
 39 | 
 40 |     One thread per topic.  Maintains a cache of recently loaded messages.
 41 |     """
 42 |     def __init__(self, timeline, topic):
 43 |         threading.Thread.__init__(self)
 44 | 
 45 |         self.timeline = timeline
 46 |         self.topic = topic
 47 | 
 48 |         self.topic_playhead_position = None
 49 | 
 50 |         self._message_cache_capacity = 50
 51 |         self._message_cache = {}
 52 |         self._message_cache_keys = []
 53 | 
 54 |         self._stop_flag = False
 55 | 
 56 |         self.setDaemon(True)
 57 |         self.start()
 58 | 
 59 |     def reset(self):
 60 |         self.bag_playhead_position = None
 61 | 
 62 |     def run(self):
 63 |         while not self._stop_flag:
 64 |             # Wait for a new entry
 65 |             cv = self.timeline._playhead_positions_cvs[self.topic]
 66 |             with cv:
 67 |                 while (self.topic not in self.timeline._playhead_positions) or (self.topic_playhead_position == self.timeline._playhead_positions[self.topic]):
 68 |                     cv.wait()
 69 |                     if self._stop_flag:
 70 |                         return
 71 |                 playhead_position = self.timeline._playhead_positions[self.topic]
 72 | 
 73 |             self.topic_playhead_position = playhead_position
 74 | 
 75 |             # Don't bother loading the message if there are no listeners
 76 |             if not self.timeline.has_listeners(self.topic):
 77 |                 continue
 78 | 
 79 |             # Load the message
 80 |             if playhead_position is None:
 81 |                 msg_data = None
 82 |             else:
 83 |                 msg_data = self._get_message(playhead_position)
 84 | 
 85 |             # Inform the views
 86 |             messages_cv = self.timeline._messages_cvs[self.topic]
 87 |             with messages_cv:
 88 |                 self.timeline._messages[self.topic] = msg_data
 89 |                 messages_cv.notify_all()      # notify all views that a message is loaded
 90 | 
 91 |     def _get_message(self, position):
 92 |         key = str(position)
 93 |         if key in self._message_cache:
 94 |             return self._message_cache[key]
 95 | 
 96 |         msg_data = self.timeline.read_message(self.topic, position)
 97 | 
 98 |         self._message_cache[key] = msg_data
 99 |         self._message_cache_keys.append(key)
100 | 
101 |         if len(self._message_cache) > self._message_cache_capacity:
102 |             oldest_key = self._message_cache_keys[0]
103 |             del self._message_cache[oldest_key]
104 |             self._message_cache_keys.remove(oldest_key)
105 | 
106 |         return msg_data
107 | 
108 |     def stop(self):
109 |         self._stop_flag = True
110 |         cv = self.timeline._playhead_positions_cvs[self.topic]
111 |         with cv:
112 |             print("DJS: self.timeline._playhead_positions_cvs[self.topic].notify_all() [MessageLoader:stop")
113 |             cv.notify_all()
114 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/topic_message_view.py:
--------------------------------------------------------------------------------
  1 | # Software License Agreement (BSD License)
  2 | #
  3 | # Copyright (c) 2012, Willow Garage, Inc.
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions
  8 | # are met:
  9 | #
 10 | #  * Redistributions of source code must retain the above copyright
 11 | #    notice, this list of conditions and the following disclaimer.
 12 | #  * Redistributions in binary form must reproduce the above
 13 | #    copyright notice, this list of conditions and the following
 14 | #    disclaimer in the documentation and/or other materials provided
 15 | #    with the distribution.
 16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
 17 | #    contributors may be used to endorse or promote products derived
 18 | #    from this software without specific prior written permission.
 19 | #
 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 31 | # POSSIBILITY OF SUCH DAMAGE.
 32 | from .message_view import MessageView
 33 | 
 34 | from python_qt_binding.QtGui import QIcon
 35 | try:  # indigo
 36 |     from python_qt_binding.QtGui import QAction, QToolBar
 37 | except ImportError:  # kinetic+ (pyqt5)
 38 |     from python_qt_binding.QtWidgets import QAction, QToolBar
 39 | 
 40 | 
 41 | class TopicMessageView(MessageView):
 42 |     """
 43 |     A message view with a toolbar for navigating messages in a single topic.
 44 |     """
 45 |     def __init__(self, timeline, parent, topic):
 46 |         MessageView.__init__(self, timeline, topic)
 47 | 
 48 |         self._parent = parent
 49 |         self._stamp = None
 50 |         self._name = parent.objectName()
 51 | 
 52 |         self.toolbar = QToolBar()
 53 |         self._first_action = QAction(QIcon.fromTheme('go-first'), '', self.toolbar)
 54 |         self._first_action.triggered.connect(self.navigate_first)
 55 |         self.toolbar.addAction(self._first_action)
 56 |         self._prev_action = QAction(QIcon.fromTheme('go-previous'), '', self.toolbar)
 57 |         self._prev_action.triggered.connect(self.navigate_previous)
 58 |         self.toolbar.addAction(self._prev_action)
 59 |         self._next_action = QAction(QIcon.fromTheme('go-next'), '', self.toolbar)
 60 |         self._next_action.triggered.connect(self.navigate_next)
 61 |         self.toolbar.addAction(self._next_action)
 62 |         self._last_action = QAction(QIcon.fromTheme('go-last'), '', self.toolbar)
 63 |         self._last_action.triggered.connect(self.navigate_last)
 64 |         self.toolbar.addAction(self._last_action)
 65 |         parent.layout().addWidget(self.toolbar)
 66 | 
 67 |     @property
 68 |     def parent(self):
 69 |         return self._parent
 70 | 
 71 |     @property
 72 |     def stamp(self):
 73 |         return self._stamp
 74 | 
 75 |     # MessageView implementation
 76 | 
 77 |     def message_viewed(self, bag, msg_details):
 78 |         _, _, self._stamp = msg_details[:3]
 79 | 
 80 |     # Events
 81 |     def navigate_first(self):
 82 |         for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region):
 83 |             self.timeline._timeline_frame.playhead = entry.time
 84 |             break
 85 | 
 86 |     def navigate_previous(self):
 87 |         last_entry = None
 88 |         for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.start_stamp, self.timeline._timeline_frame.playhead):
 89 |             if entry.time < self.timeline._timeline_frame.playhead:
 90 |                 last_entry = entry
 91 | 
 92 |         if last_entry:
 93 |             self.timeline._timeline_frame.playhead = last_entry.time
 94 | 
 95 |     def navigate_next(self):
 96 |         for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.playhead, self.timeline._timeline_frame.end_stamp):
 97 |             if entry.time > self.timeline._timeline_frame.playhead:
 98 |                 self.timeline._timeline_frame.playhead = entry.time
 99 |                 break
100 | 
101 |     def navigate_last(self):
102 |         last_entry = None
103 |         for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region):
104 |             last_entry = entry
105 | 
106 |         if last_entry:
107 |             self.timeline._timeline_frame.playhead = last_entry.time
108 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/pydotfactory.py:
--------------------------------------------------------------------------------
  1 | #
  2 | # License: Yujin
  3 | #
  4 | ##############################################################################
  5 | # Description
  6 | ##############################################################################
  7 | 
  8 | """
  9 | .. module:: pydotfactory
 10 |    :platform: Unix
 11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.pydotfactory module.
 12 | 
 13 | Oh my spaghettified magnificence,
 14 | Bless my noggin with a tickle from your noodly appendages!
 15 | 
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | from distutils.version import LooseVersion
 23 | import urllib
 24 | 
 25 | import pydot
 26 | # work around for https://bugs.launchpad.net/ubuntu/+source/pydot/+bug/1321135
 27 | import pyparsing
 28 | pyparsing._noncomma = "".join([c for c in pyparsing.printables if c != ","])
 29 | 
 30 | ##############################################################################
 31 | # Classes
 32 | ##############################################################################
 33 | 
 34 | 
 35 | # Reference implementation for a dotcode factory
 36 | class PydotFactory():
 37 | 
 38 |     def __init__(self):
 39 |         pass
 40 | 
 41 |     def escape_label(self, name):
 42 |         if name in ['graph', 'subgraph', 'node', 'edge']:
 43 |             ret = "%s_" % name
 44 |         else:
 45 |             ret = name
 46 |         return ret
 47 | 
 48 |     def escape_name(self, name):
 49 |         ret = urllib.quote(name.strip())
 50 |         ret = ret.replace('/', '_')
 51 |         ret = ret.replace('%', '_')
 52 |         ret = ret.replace('-', '_')
 53 |         return self.escape_label(ret)
 54 | 
 55 |     def get_graph(self, graph_type='digraph', rank='same', simplify=True, rankdir='TB', ranksep=0.2, compound=True):
 56 |         # Lucid version of pydot bugs with certain settings, not sure which version exactly fixes those
 57 |         if LooseVersion(pydot.__version__) > LooseVersion('1.0.10'):
 58 |             graph = pydot.Dot('graphname',
 59 |                               graph_type=graph_type,
 60 |                               rank=rank,
 61 |                               rankdir=rankdir,
 62 |                               simplify=simplify
 63 |                               )
 64 |             graph.set_ranksep(ranksep)
 65 |             graph.set_compound(compound)
 66 |         else:
 67 |             graph = pydot.Dot('graphname',
 68 |                               graph_type=graph_type,
 69 |                               rank=rank,
 70 |                               rankdir=rankdir)
 71 |         return graph
 72 | 
 73 |     def add_node_to_graph(self,
 74 |                           graph,
 75 |                           nodename,
 76 |                           nodelabel=None,
 77 |                           shape='box',
 78 |                           color=None,
 79 |                           url=None,
 80 |                           tooltip=None):
 81 |         """
 82 |         creates a node item for this factory, adds it to the graph.
 83 |         Node name can vary from label but must always be same for the same node label
 84 |         """
 85 |         if nodename is None or nodename == '':
 86 |             raise ValueError('Empty Node name')
 87 |         if nodelabel is None:
 88 |             nodelabel = nodename
 89 |         node = pydot.Node(self.escape_name(nodename))
 90 |         node.set_shape(shape)
 91 |         node.set_label(self.escape_label(nodelabel))
 92 |         if tooltip is not None:
 93 |             node.set_tooltip(tooltip)
 94 |         if url is not None:
 95 |             node.set_URL(self.escape_name(url))
 96 |         if color is not None:
 97 |             node.set_color(color)
 98 |         graph.add_node(node)
 99 | 
100 |     def add_subgraph_to_graph(self,
101 |                               graph,
102 |                               subgraphname,
103 |                               rank='same',
104 |                               simplify=True,
105 |                               rankdir='TB',
106 |                               ranksep=0.2,
107 |                               compound=True,
108 |                               color=None,
109 |                               shape='box',
110 |                               style='bold',
111 |                               subgraphlabel=None):
112 |         """
113 |         creates a cluster subgraph  item for this factory, adds it to the graph.
114 |         cluster name can vary from label but must always be same for the same node label.
115 |         Most layouters require cluster names to start with cluster.
116 |         """
117 |         if subgraphname is None or subgraphname == '':
118 |             raise ValueError('Empty subgraph name')
119 |         g = pydot.Cluster(self.escape_name(subgraphname), rank=rank, rankdir=rankdir, simplify=simplify, color=color)
120 |         if 'set_style' in g.__dict__:
121 |             g.set_style(style)
122 |         if 'set_shape' in g.__dict__:
123 |             g.set_shape(shape)
124 |         if LooseVersion(pydot.__version__) > LooseVersion('1.0.10'):
125 |             g.set_compound(compound)
126 |             g.set_ranksep(ranksep)
127 |         subgraphlabel = subgraphname if subgraphlabel is None else subgraphlabel
128 |         subgraphlabel = self.escape_label(subgraphlabel)
129 |         if subgraphlabel:
130 |             g.set_label(subgraphlabel)
131 |         if 'set_color' in g.__dict__:
132 |             if color is not None:
133 |                 g.set_color(color)
134 |         graph.add_subgraph(g)
135 |         return g
136 | 
137 |     def add_edge_to_graph(self, graph, nodename1, nodename2, label=None, url=None, simplify=True, style=None, penwidth=1, color=None):
138 |         if simplify and LooseVersion(pydot.__version__) < LooseVersion('1.0.10'):
139 |             if graph.get_edge(self.escape_name(nodename1), self.escape_name(nodename2)) != []:
140 |                 return
141 |         edge = pydot.Edge(self.escape_name(nodename1), self.escape_name(nodename2))
142 |         if label is not None and label != '':
143 |             edge.set_label(label)
144 |         if url is not None:
145 |             edge.set_URL(self.escape_name(url))
146 |         if style is not None:
147 |             edge.set_style(style)
148 |         edge.obj_dict['attributes']['penwidth'] = str(penwidth)
149 |         if color is not None:
150 |             edge.obj_dict['attributes']['colorR'] = str(color[0])
151 |             edge.obj_dict['attributes']['colorG'] = str(color[1])
152 |             edge.obj_dict['attributes']['colorB'] = str(color[2])
153 |         graph.add_edge(edge)
154 | 
155 |     def create_dot(self, graph):
156 |         dot = graph.create_dot()
157 |         # sadly pydot generates line wraps cutting between numbers
158 |         return dot.replace("\\\n", "")
159 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/edge_item.py:
--------------------------------------------------------------------------------
  1 | #
  2 | # License: Yujin
  3 | #
  4 | ##############################################################################
  5 | # Description
  6 | ##############################################################################
  7 | 
  8 | """
  9 | .. module:: edge_item
 10 |    :platform: Unix
 11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.edge_item module.
 12 | 
 13 | Oh my spaghettified magnificence,
 14 | Bless my noggin with a tickle from your noodly appendages!
 15 | 
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | from python_qt_binding.QtCore import QPointF, Qt
 23 | from python_qt_binding.QtGui import QBrush, QPainterPath, QPen, QPolygonF
 24 | try:  # indigo
 25 |     from python_qt_binding.QtGui import QGraphicsPathItem, QGraphicsPolygonItem, QGraphicsSimpleTextItem
 26 | except ImportError:  # kinetic+ (pyqt5)
 27 |     from python_qt_binding.QtWidgets import QGraphicsPathItem, QGraphicsPolygonItem, QGraphicsSimpleTextItem
 28 | 
 29 | from .graph_item import GraphItem
 30 | 
 31 | ##############################################################################
 32 | # Classes
 33 | ##############################################################################
 34 | 
 35 | 
 36 | class EdgeItem(GraphItem):
 37 | 
 38 |     _qt_pen_styles = {
 39 |         'dashed': Qt.DashLine,
 40 |         'dotted': Qt.DotLine,
 41 |         'solid': Qt.SolidLine,
 42 |     }
 43 | 
 44 |     def __init__(self, highlight_level, spline, label_center, label, from_node, to_node, parent=None, penwidth=1, edge_color=None, style='solid'):
 45 |         super(EdgeItem, self).__init__(highlight_level, parent)
 46 | 
 47 |         self.from_node = from_node
 48 |         self.from_node.add_outgoing_edge(self)
 49 |         self.to_node = to_node
 50 |         self.to_node.add_incoming_edge(self)
 51 | 
 52 |         self._default_edge_color = self._COLOR_BLACK
 53 |         self._default_shape_color = self._COLOR_BLACK
 54 |         if edge_color is not None:
 55 |             self._default_edge_color = edge_color
 56 |             self._default_shape_color = edge_color
 57 | 
 58 |         self._default_text_color = self._COLOR_BLACK
 59 |         self._default_color = self._COLOR_BLACK
 60 |         self._text_brush = QBrush(self._default_color)
 61 |         self._shape_brush = QBrush(self._default_color)
 62 |         if style in ['dashed', 'dotted']:
 63 |             self._shape_brush = QBrush(Qt.transparent)
 64 |         self._label_pen = QPen()
 65 |         self._label_pen.setColor(self._default_text_color)
 66 |         self._label_pen.setJoinStyle(Qt.RoundJoin)
 67 |         self._edge_pen = QPen(self._label_pen)
 68 |         self._edge_pen.setWidth(penwidth)
 69 |         self._edge_pen.setColor(self._default_edge_color)
 70 |         self._edge_pen.setStyle(self._qt_pen_styles.get(style, Qt.SolidLine))
 71 |         self._sibling_edges = set()
 72 | 
 73 |         self._label = None
 74 |         if label is not None:
 75 |             self._label = QGraphicsSimpleTextItem(label)
 76 |             label_rect = self._label.boundingRect()
 77 |             label_rect.moveCenter(label_center)
 78 |             self._label.setPos(label_rect.x(), label_rect.y())
 79 |             self._label.hoverEnterEvent = self._handle_hoverEnterEvent
 80 |             self._label.hoverLeaveEvent = self._handle_hoverLeaveEvent
 81 |             self._label.setAcceptHoverEvents(True)
 82 | 
 83 |         # spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
 84 |         coordinates = spline.split(' ')
 85 |         # extract optional end_point
 86 |         end_point = None
 87 |         if (coordinates[0].startswith('e,')):
 88 |             parts = coordinates.pop(0)[2:].split(',')
 89 |             end_point = QPointF(float(parts[0]), -float(parts[1]))
 90 |         # extract optional start_point
 91 |         if (coordinates[0].startswith('s,')):
 92 |             parts = coordinates.pop(0).split(',')
 93 | 
 94 |         # first point
 95 |         parts = coordinates.pop(0).split(',')
 96 |         point = QPointF(float(parts[0]), -float(parts[1]))
 97 |         path = QPainterPath(point)
 98 | 
 99 |         while len(coordinates) > 2:
100 |             # extract triple of points for a cubic spline
101 |             parts = coordinates.pop(0).split(',')
102 |             point1 = QPointF(float(parts[0]), -float(parts[1]))
103 |             parts = coordinates.pop(0).split(',')
104 |             point2 = QPointF(float(parts[0]), -float(parts[1]))
105 |             parts = coordinates.pop(0).split(',')
106 |             point3 = QPointF(float(parts[0]), -float(parts[1]))
107 |             path.cubicTo(point1, point2, point3)
108 | 
109 |         self._arrow = None
110 |         if end_point is not None:
111 |             # draw arrow
112 |             self._arrow = QGraphicsPolygonItem()
113 |             polygon = QPolygonF()
114 |             polygon.append(point3)
115 |             offset = QPointF(end_point - point3)
116 |             corner1 = QPointF(-offset.y(), offset.x()) * 0.35
117 |             corner2 = QPointF(offset.y(), -offset.x()) * 0.35
118 |             polygon.append(point3 + corner1)
119 |             polygon.append(end_point)
120 |             polygon.append(point3 + corner2)
121 |             self._arrow.setPolygon(polygon)
122 |             self._arrow.hoverEnterEvent = self._handle_hoverEnterEvent
123 |             self._arrow.hoverLeaveEvent = self._handle_hoverLeaveEvent
124 |             self._arrow.setAcceptHoverEvents(True)
125 | 
126 |         self._path = QGraphicsPathItem()
127 |         self._path.setPath(path)
128 |         self.addToGroup(self._path)
129 | 
130 |         self.set_node_color()
131 |         self.set_label_color()
132 | 
133 |     def add_to_scene(self, scene):
134 |         scene.addItem(self)
135 |         if self._label is not None:
136 |             scene.addItem(self._label)
137 |         if self._arrow is not None:
138 |             scene.addItem(self._arrow)
139 | 
140 |     def setToolTip(self, tool_tip):
141 |         super(EdgeItem, self).setToolTip(tool_tip)
142 |         if self._label is not None:
143 |             self._label.setToolTip(tool_tip)
144 |         if self._arrow is not None:
145 |             self._arrow.setToolTip(tool_tip)
146 | 
147 |     def add_sibling_edge(self, edge):
148 |         self._sibling_edges.add(edge)
149 | 
150 |     def set_node_color(self, color=None):
151 |         if color is None:
152 |             self._label_pen.setColor(self._default_text_color)
153 |             self._text_brush.setColor(self._default_color)
154 |             if self._shape_brush.isOpaque():
155 |                 self._shape_brush.setColor(self._default_shape_color)
156 |             self._edge_pen.setColor(self._default_edge_color)
157 |         else:
158 |             self._label_pen.setColor(color)
159 |             self._text_brush.setColor(color)
160 |             if self._shape_brush.isOpaque():
161 |                 self._shape_brush.setColor(color)
162 |             self._edge_pen.setColor(color)
163 | 
164 |         self._path.setPen(self._edge_pen)
165 |         if self._arrow is not None:
166 |             self._arrow.setBrush(self._shape_brush)
167 |             self._arrow.setPen(self._edge_pen)
168 | 
169 |     def set_label_color(self, color=None):
170 |         if color is None:
171 |             self._label_pen.setColor(self._default_text_color)
172 |         else:
173 |             self._label_pen.setColor(color)
174 | 
175 |         if self._label is not None:
176 |             self._label.setBrush(self._text_brush)
177 |             self._label.setPen(self._label_pen)
178 | 
179 |     def _handle_hoverEnterEvent(self, event):
180 |         # hovered edge item in red
181 |         self.set_node_color(self._COLOR_RED)
182 | 
183 |         if self._highlight_level > 1:
184 |             if self.from_node != self.to_node:
185 |                 # from-node in blue
186 |                 self.from_node.set_node_color(self._COLOR_BLUE)
187 |                 # to-node in green
188 |                 self.to_node.set_node_color(self._COLOR_GREEN)
189 |             else:
190 |                 # from-node/in-node in teal
191 |                 self.from_node.set_node_color(self._COLOR_TEAL)
192 |                 self.to_node.set_node_color(self._COLOR_TEAL)
193 |         if self._highlight_level > 2:
194 |             # sibling edges in orange
195 |             for sibling_edge in self._sibling_edges:
196 |                 sibling_edge.set_node_color(self._COLOR_ORANGE)
197 | 
198 |     def _handle_hoverLeaveEvent(self, event):
199 |         self.set_node_color()
200 |         if self._highlight_level > 1:
201 |             self.from_node.set_node_color()
202 |             self.to_node.set_node_color()
203 |         if self._highlight_level > 2:
204 |             for sibling_edge in self._sibling_edges:
205 |                 sibling_edge.set_node_color()
206 | 


--------------------------------------------------------------------------------
/resource/RosBehaviourTree.ui:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <ui version="4.0">
  3 |  <class>RosBehaviourTreeWidget</class>
  4 |  <widget class="QWidget" name="RosBehaviourTreeWidget">
  5 |   <property name="enabled">
  6 |    <bool>true</bool>
  7 |   </property>
  8 |   <property name="geometry">
  9 |    <rect>
 10 |     <x>0</x>
 11 |     <y>0</y>
 12 |     <width>1144</width>
 13 |     <height>503</height>
 14 |    </rect>
 15 |   </property>
 16 |   <property name="windowTitle">
 17 |    <string>PyTrees Behaviour Tree </string>
 18 |   </property>
 19 |   <layout class="QVBoxLayout" name="verticalLayout" stretch="0,0,0">
 20 |    <item>
 21 |     <layout class="QHBoxLayout" name="top_bar_layout" stretch="0,0,0,0,0,0,0,0,0,0,0,0,0,0,0">
 22 |      <item>
 23 |       <widget class="QComboBox" name="topic_combo_box">
 24 |        <property name="sizePolicy">
 25 |         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
 26 |          <horstretch>0</horstretch>
 27 |          <verstretch>0</verstretch>
 28 |         </sizepolicy>
 29 |        </property>
 30 |        <property name="minimumSize">
 31 |         <size>
 32 |          <width>170</width>
 33 |          <height>0</height>
 34 |         </size>
 35 |        </property>
 36 |        <property name="baseSize">
 37 |         <size>
 38 |          <width>0</width>
 39 |          <height>0</height>
 40 |         </size>
 41 |        </property>
 42 |        <property name="editable">
 43 |         <bool>false</bool>
 44 |        </property>
 45 |        <property name="sizeAdjustPolicy">
 46 |         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
 47 |        </property>
 48 |       </widget>
 49 |      </item>
 50 |      <item>
 51 |       <widget class="QCheckBox" name="highlight_connections_check_box">
 52 |        <property name="toolTip">
 53 |         <string>Highlight incoming and outgoing connections on mouse over</string>
 54 |        </property>
 55 |        <property name="text">
 56 |         <string>Highlight</string>
 57 |        </property>
 58 |        <property name="checked">
 59 |         <bool>true</bool>
 60 |        </property>
 61 |       </widget>
 62 |      </item>
 63 |      <item>
 64 |       <widget class="QCheckBox" name="auto_fit_graph_check_box">
 65 |        <property name="toolTip">
 66 |         <string>Automatically fit graph into view on update</string>
 67 |        </property>
 68 |        <property name="text">
 69 |         <string>Fit</string>
 70 |        </property>
 71 |        <property name="checked">
 72 |         <bool>true</bool>
 73 |        </property>
 74 |       </widget>
 75 |      </item>
 76 |      <item>
 77 |       <widget class="QPushButton" name="fit_in_view_push_button">
 78 |        <property name="toolTip">
 79 |         <string>Fit graph in view</string>
 80 |        </property>
 81 |       </widget>
 82 |      </item>
 83 |      <item>
 84 |       <widget class="QToolButton" name="play_tool_button">
 85 |        <property name="text">
 86 |         <string>Play</string>
 87 |        </property>
 88 |       </widget>
 89 |      </item>
 90 |      <item>
 91 |       <widget class="QToolButton" name="stop_tool_button">
 92 |        <property name="text">
 93 |         <string>Stop</string>
 94 |        </property>
 95 |       </widget>
 96 |      </item>
 97 |      <item>
 98 |       <widget class="QComboBox" name="visibility_level_combo_box"/>
 99 |      </item>
100 |      <item>
101 |       <spacer name="horizontalSpacer">
102 |        <property name="orientation">
103 |         <enum>Qt::Horizontal</enum>
104 |        </property>
105 |        <property name="sizeHint" stdset="0">
106 |         <size>
107 |          <width>40</width>
108 |          <height>20</height>
109 |         </size>
110 |        </property>
111 |       </spacer>
112 |      </item>
113 |      <item>
114 |       <widget class="QLabel" name="message_label">
115 |        <property name="font">
116 |         <font>
117 |          <pointsize>15</pointsize>
118 |         </font>
119 |        </property>
120 |        <property name="text">
121 |         <string/>
122 |        </property>
123 |       </widget>
124 |      </item>
125 |      <item>
126 |       <spacer name="horizontalSpacer_3">
127 |        <property name="orientation">
128 |         <enum>Qt::Horizontal</enum>
129 |        </property>
130 |        <property name="sizeHint" stdset="0">
131 |         <size>
132 |          <width>40</width>
133 |          <height>20</height>
134 |         </size>
135 |        </property>
136 |       </spacer>
137 |      </item>
138 |      <item>
139 |       <widget class="QPushButton" name="load_bag_push_button">
140 |        <property name="toolTip">
141 |         <string>Load and view trees from a .bag file</string>
142 |        </property>
143 |        <property name="text">
144 |         <string/>
145 |        </property>
146 |       </widget>
147 |      </item>
148 |      <item>
149 |       <widget class="QPushButton" name="load_dot_push_button">
150 |        <property name="toolTip">
151 |         <string>Load DOT graph from file</string>
152 |        </property>
153 |       </widget>
154 |      </item>
155 |      <item>
156 |       <widget class="QPushButton" name="save_dot_push_button">
157 |        <property name="toolTip">
158 |         <string>Save as DOT graph</string>
159 |        </property>
160 |       </widget>
161 |      </item>
162 |      <item>
163 |       <widget class="QPushButton" name="save_as_svg_push_button">
164 |        <property name="toolTip">
165 |         <string>Save as SVG</string>
166 |        </property>
167 |       </widget>
168 |      </item>
169 |      <item>
170 |       <widget class="QPushButton" name="save_as_image_push_button">
171 |        <property name="toolTip">
172 |         <string>Save as image</string>
173 |        </property>
174 |       </widget>
175 |      </item>
176 |     </layout>
177 |    </item>
178 |    <item>
179 |     <widget class="InteractiveGraphicsView" name="graphics_view">
180 |      <property name="renderHints">
181 |       <set>QPainter::Antialiasing|QPainter::HighQualityAntialiasing|QPainter::SmoothPixmapTransform|QPainter::TextAntialiasing</set>
182 |      </property>
183 |      <property name="resizeAnchor">
184 |       <enum>QGraphicsView::AnchorViewCenter</enum>
185 |      </property>
186 |     </widget>
187 |    </item>
188 |    <item>
189 |     <layout class="QHBoxLayout" name="bottom_bar_layout">
190 |      <item>
191 |       <widget class="QToolButton" name="first_tool_button">
192 |        <property name="toolTip">
193 |         <string>Go to the first tree</string>
194 |        </property>
195 |        <property name="text">
196 |         <string>First</string>
197 |        </property>
198 |       </widget>
199 |      </item>
200 |      <item>
201 |       <widget class="QToolButton" name="previous_tool_button">
202 |        <property name="toolTip">
203 |         <string>View the previous tree in the sequence</string>
204 |        </property>
205 |        <property name="text">
206 |         <string>Previous</string>
207 |        </property>
208 |       </widget>
209 |      </item>
210 |      <item>
211 |       <layout class="QVBoxLayout" name="timeline_layout">
212 |        <property name="leftMargin">
213 |         <number>0</number>
214 |        </property>
215 |        <item>
216 |         <widget class="QGraphicsView" name="timeline_graphics_view">
217 |          <property name="sizePolicy">
218 |           <sizepolicy hsizetype="Expanding" vsizetype="Minimum">
219 |            <horstretch>0</horstretch>
220 |            <verstretch>0</verstretch>
221 |           </sizepolicy>
222 |          </property>
223 |          <property name="minimumSize">
224 |           <size>
225 |            <width>0</width>
226 |            <height>0</height>
227 |           </size>
228 |          </property>
229 |          <property name="maximumSize">
230 |           <size>
231 |            <width>16777215</width>
232 |            <height>60</height>
233 |           </size>
234 |          </property>
235 |          <property name="mouseTracking">
236 |           <bool>false</bool>
237 |          </property>
238 |          <property name="alignment">
239 |           <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
240 |          </property>
241 |         </widget>
242 |        </item>
243 |       </layout>
244 |      </item>
245 |      <item>
246 |       <widget class="QToolButton" name="next_tool_button">
247 |        <property name="toolTip">
248 |         <string>View the next tree in the sequence</string>
249 |        </property>
250 |        <property name="layoutDirection">
251 |         <enum>Qt::LeftToRight</enum>
252 |        </property>
253 |        <property name="text">
254 |         <string>Next</string>
255 |        </property>
256 |       </widget>
257 |      </item>
258 |      <item>
259 |       <widget class="QToolButton" name="last_tool_button">
260 |        <property name="toolTip">
261 |         <string>Go to the latest tree</string>
262 |        </property>
263 |        <property name="text">
264 |         <string>Last</string>
265 |        </property>
266 |       </widget>
267 |      </item>
268 |     </layout>
269 |    </item>
270 |   </layout>
271 |  </widget>
272 |  <customwidgets>
273 |   <customwidget>
274 |    <class>InteractiveGraphicsView</class>
275 |    <extends>QGraphicsView</extends>
276 |    <header>rqt_graph.interactive_graphics_view</header>
277 |   </customwidget>
278 |  </customwidgets>
279 |  <resources/>
280 |  <connections/>
281 | </ui>
282 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/plugins/raw_view.py:
--------------------------------------------------------------------------------
  1 | # Software License Agreement (BSD License)
  2 | #
  3 | # Copyright (c) 2012, Willow Garage, Inc.
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions
  8 | # are met:
  9 | #
 10 | #  * Redistributions of source code must retain the above copyright
 11 | #    notice, this list of conditions and the following disclaimer.
 12 | #  * Redistributions in binary form must reproduce the above
 13 | #    copyright notice, this list of conditions and the following
 14 | #    disclaimer in the documentation and/or other materials provided
 15 | #    with the distribution.
 16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
 17 | #    contributors may be used to endorse or promote products derived
 18 | #    from this software without specific prior written permission.
 19 | #
 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 31 | # POSSIBILITY OF SUCH DAMAGE.
 32 | """
 33 | Defines a raw view: a TopicMessageView that displays the message contents in a tree.
 34 | """
 35 | import rospy
 36 | import codecs
 37 | import math
 38 | 
 39 | from python_qt_binding.QtCore import Qt
 40 | try:  # indigo
 41 |     from python_qt_binding.QtGui import QApplication, QAbstractItemView, QSizePolicy, QTreeWidget, QTreeWidgetItem
 42 | except ImportError:  # kinetic+ (pyqt5)
 43 |     from python_qt_binding.QtWidgets import QApplication, QAbstractItemView, QSizePolicy, QTreeWidget, QTreeWidgetItem
 44 | from .topic_message_view import TopicMessageView
 45 | 
 46 | 
 47 | class RawView(TopicMessageView):
 48 |     name = 'Raw'
 49 |     """
 50 |     Plugin to view a message in a treeview window
 51 |     The message is loaded into a custum treewidget
 52 |     """
 53 |     def __init__(self, timeline, parent, topic):
 54 |         """
 55 |         :param timeline: timeline data object, ''BagTimeline''
 56 |         :param parent: widget that will be added to the ros_gui context, ''QWidget''
 57 |         """
 58 |         super(RawView, self).__init__(timeline, parent, topic)
 59 |         self.message_tree = MessageTree(parent)
 60 |         parent.layout().addWidget(self.message_tree)  # This will automatically resize the message_tree to the windowsize
 61 | 
 62 |     def message_viewed(self, bag, msg_details):
 63 |         super(RawView, self).message_viewed(bag, msg_details)
 64 |         _, msg, t = msg_details  # topic, msg, t = msg_details
 65 |         if t is None:
 66 |             self.message_cleared()
 67 |         else:
 68 |             self.message_tree.set_message(msg)
 69 | 
 70 |     def message_cleared(self):
 71 |         TopicMessageView.message_cleared(self)
 72 |         self.message_tree.set_message(None)
 73 | 
 74 | 
 75 | class MessageTree(QTreeWidget):
 76 |     def __init__(self, parent):
 77 |         super(MessageTree, self).__init__(parent)
 78 |         self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
 79 |         self.setHeaderHidden(True)
 80 |         self.setSelectionMode(QAbstractItemView.ExtendedSelection)
 81 |         self._msg = None
 82 | 
 83 |         self._expanded_paths = None
 84 |         self.keyPressEvent = self.on_key_press
 85 | 
 86 |     @property
 87 |     def msg(self):
 88 |         return self._msg
 89 | 
 90 |     def set_message(self, msg):
 91 |         """
 92 |         Clears the tree view and displays the new message
 93 |         :param msg: message object to display in the treeview, ''msg''
 94 |         """
 95 |         # Remember whether items were expanded or not before deleting
 96 |         if self._msg:
 97 |             for item in self.get_all_items():
 98 |                 path = self.get_item_path(item)
 99 |                 if item.isExpanded():
100 |                     self._expanded_paths.add(path)
101 |                 elif path in self._expanded_paths:
102 |                     self._expanded_paths.remove(path)
103 |             self.clear()
104 |         if msg:
105 |             # Populate the tree
106 |             self._add_msg_object(None, '', '', msg, msg._type)
107 | 
108 |             if self._expanded_paths is None:
109 |                 self._expanded_paths = set()
110 |             else:
111 |                 # Expand those that were previously expanded, and collapse any paths that we've seen for the first time
112 |                 for item in self.get_all_items():
113 |                     path = self.get_item_path(item)
114 |                     if path in self._expanded_paths:
115 |                         item.setExpanded(True)
116 |                     else:
117 |                         item.setExpanded(False)
118 |         self._msg = msg
119 |         self.update()
120 | 
121 |     # Keyboard handler
122 |     def on_key_press(self, event):
123 |         key, ctrl = event.key(), event.modifiers() & Qt.ControlModifier
124 |         if ctrl:
125 |             if key == ord('C') or key == ord('c'):
126 |                 # Ctrl-C: copy text from selected items to clipboard
127 |                 self._copy_text_to_clipboard()
128 |                 event.accept()
129 |             elif key == ord('A') or key == ord('a'):
130 |                 # Ctrl-A: select all
131 |                 self._select_all()
132 | 
133 |     def _select_all(self):
134 |         for i in self.get_all_items():
135 |             if not i.isSelected():
136 |                 i.setSelected(True)
137 |                 i.setExpanded(True)
138 | 
139 |     def _copy_text_to_clipboard(self):
140 |         # Get tab indented text for all selected items
141 |         def get_distance(item, ancestor, distance=0):
142 |             parent = item.parent()
143 |             if parent == None:
144 |                 return distance
145 |             else:
146 |                 return get_distance(parent, ancestor, distance + 1)
147 |         text = ''
148 |         for i in self.get_all_items():
149 |             if i in self.selectedItems():
150 |                 text += ('\t' * (get_distance(i, None))) + i.text(0) + '\n'
151 |         # Copy the text to the clipboard
152 |         clipboard = QApplication.clipboard()
153 |         clipboard.setText(text)
154 | 
155 |     def get_item_path(self, item):
156 |         return item.data(0, Qt.UserRole)[0].replace(' ', '')  # remove spaces that may get introduced in indexing, e.g. [  3] is [3]
157 | 
158 |     def get_all_items(self):
159 |         items = []
160 |         try:
161 |             root = self.invisibleRootItem()
162 |             self.traverse(root, items.append)
163 |         except Exception:
164 |             # TODO: very large messages can cause a stack overflow due to recursion
165 |             pass
166 |         return items
167 | 
168 |     def traverse(self, root, function):
169 |         for i in range(root.childCount()):
170 |             child = root.child(i)
171 |             function(child)
172 |             self.traverse(child, function)
173 | 
174 |     def _add_msg_object(self, parent, path, name, obj, obj_type):
175 |         label = name
176 | 
177 |         if hasattr(obj, '__slots__'):
178 |             subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__]
179 |         elif type(obj) in [list, tuple]:
180 |             len_obj = len(obj)
181 |             if len_obj == 0:
182 |                 subobjs = []
183 |             else:
184 |                 w = int(math.ceil(math.log10(len_obj)))
185 |                 subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)]
186 |         else:
187 |             subobjs = []
188 | 
189 |         if type(obj) in [int, long, float]:
190 |             if type(obj) == float:
191 |                 obj_repr = '%.6f' % obj
192 |             else:
193 |                 obj_repr = str(obj)
194 | 
195 |             if obj_repr[0] == '-':
196 |                 label += ': %s' % obj_repr
197 |             else:
198 |                 label += ':  %s' % obj_repr
199 | 
200 |         elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]:
201 |             # Ignore any binary data
202 |             obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0]
203 | 
204 |             # Truncate long representations
205 |             if len(obj_repr) >= 50:
206 |                 obj_repr = obj_repr[:50] + '...'
207 | 
208 |             label += ': ' + obj_repr
209 |         item = QTreeWidgetItem([label])
210 |         if name == '':
211 |             pass
212 |         elif path.find('.') == -1 and path.find('[') == -1:
213 |             self.addTopLevelItem(item)
214 |         else:
215 |             parent.addChild(item)
216 |         item.setData(0, Qt.UserRole, (path, obj_type))
217 | 
218 |         for subobj_name, subobj in subobjs:
219 |             if subobj is None:
220 |                 continue
221 | 
222 |             if path == '':
223 |                 subpath = subobj_name  # root field
224 |             elif subobj_name.startswith('['):
225 |                 subpath = '%s%s' % (path, subobj_name)  # list, dict, or tuple
226 |             else:
227 |                 subpath = '%s.%s' % (path, subobj_name)  # attribute (prefix with '.')
228 | 
229 |             if hasattr(subobj, '_type'):
230 |                 subobj_type = subobj._type
231 |             else:
232 |                 subobj_type = type(subobj).__name__
233 | 
234 |             self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type)
235 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/dotcode_behaviour.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | #
  3 | # License: BSD
  4 | #   https://raw.github.com/stonier/rqt_py_trees/license/LICENSE
  5 | #
  6 | ##############################################################################
  7 | # Documentation
  8 | ##############################################################################
  9 | 
 10 | """
 11 | .. module:: dotcode_behaviour
 12 |    :platform: Unix
 13 |    :synopsis: Dot code a behaviour tree
 14 | 
 15 | Dotcode generation for a py_trees behaviour tree.
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | from __future__ import with_statement, print_function
 23 | 
 24 | import rospy
 25 | import py_trees_msgs.msg as py_trees_msgs
 26 | import unique_id
 27 | 
 28 | ##############################################################################
 29 | # Classes
 30 | ##############################################################################
 31 | 
 32 | 
 33 | class RosBehaviourTreeDotcodeGenerator(object):
 34 | 
 35 |     def __init__(self):
 36 |         self.last_drawargs = None
 37 |         self.dotcode = None
 38 |         self.firstcall = True
 39 |         self.rank = None
 40 |         self.rankdir = None
 41 |         self.ranksep = None
 42 |         self.graph = None
 43 |         self.dotcode_factory = None
 44 |         self.active_and_status_colour_hex_map = {
 45 |             (False, py_trees_msgs.Behaviour.INVALID): '#e4e4e4',
 46 |             (True, py_trees_msgs.Behaviour.INVALID): '#e4e4e4',
 47 |             (False, py_trees_msgs.Behaviour.RUNNING): '#9696c8',
 48 |             (True, py_trees_msgs.Behaviour.RUNNING): '#0000ff',
 49 |             (False, py_trees_msgs.Behaviour.FAILURE): '#c89696',
 50 |             (True, py_trees_msgs.Behaviour.FAILURE): '#ff0000',
 51 |             (False, py_trees_msgs.Behaviour.SUCCESS): '#96c896',
 52 |             (True, py_trees_msgs.Behaviour.SUCCESS): '#00ff00',
 53 |         }
 54 |         self.active_and_status_colour_tuple_map = {
 55 |             (False, py_trees_msgs.Behaviour.INVALID): (228, 228, 228),
 56 |             (True, py_trees_msgs.Behaviour.INVALID): (228, 228, 228),
 57 |             (False, py_trees_msgs.Behaviour.RUNNING): (150, 150, 200),
 58 |             (True, py_trees_msgs.Behaviour.RUNNING): (0, 0, 255),
 59 |             (False, py_trees_msgs.Behaviour.FAILURE): (200, 150, 150),
 60 |             (True, py_trees_msgs.Behaviour.FAILURE): (255, 0, 0),
 61 |             (False, py_trees_msgs.Behaviour.SUCCESS): (150, 200, 150),
 62 |             (True, py_trees_msgs.Behaviour.SUCCESS): (0, 255, 0),
 63 |         }
 64 | 
 65 |     def generate_dotcode(self,
 66 |                          dotcode_factory,
 67 |                          timer=rospy.Time,
 68 |                          behaviours=None,
 69 |                          timestamp=None,
 70 |                          rank='same',   # None, same, min, max, source, sink
 71 |                          ranksep=0.2,   # vertical distance between layers
 72 |                          rankdir='TB',  # direction of layout (TB top > bottom, LR left > right)
 73 |                          force_refresh=False):
 74 |         """
 75 |         :param py_trees_msgs.Behaviour[] behaviours:
 76 |         :param force_refresh: if False, may return same dotcode as last time
 77 |         """
 78 |         if self.firstcall is True:
 79 |             self.firstcall = False
 80 |             force_refresh = True
 81 | 
 82 |         drawing_args = {
 83 |             'dotcode_factory': dotcode_factory,
 84 |             "rank": rank,
 85 |             "rankdir": rankdir,
 86 |             "ranksep": ranksep}
 87 | 
 88 |         selection_changed = False
 89 |         if self.last_drawargs != drawing_args:
 90 |             selection_changed = True
 91 |             self.last_drawargs = drawing_args
 92 | 
 93 |             self.dotcode_factory = dotcode_factory
 94 |             self.rank = rank
 95 |             self.rankdir = rankdir
 96 |             self.ranksep = ranksep
 97 | 
 98 |         self.graph = self.generate(behaviours, timestamp)
 99 |         self.dotcode = self.dotcode_factory.create_dot(self.graph)
100 |         return self.dotcode
101 | 
102 |     def type_to_shape(self, behaviour_type):
103 |         """
104 |         qt_dotgraph.node_item only supports drawing in qt of two
105 |         shapes - box, ellipse.
106 |         """
107 |         if behaviour_type == py_trees_msgs.Behaviour.BEHAVIOUR:
108 |             return 'ellipse'
109 |         elif behaviour_type == py_trees_msgs.Behaviour.SEQUENCE:
110 |             return 'box'
111 |         elif behaviour_type == py_trees_msgs.Behaviour.SELECTOR:
112 |             return 'octagon'
113 |         elif behaviour_type == py_trees_msgs.Behaviour.PARALLEL:
114 |             return 'note'
115 |         elif behaviour_type == py_trees_msgs.Behaviour.CHOOSER:
116 |             return 'doubleoctagon'
117 |         else:
118 |             return 'ellipse'
119 | 
120 |     def type_to_colour(self, behaviour_type):
121 |         if behaviour_type == py_trees_msgs.Behaviour.BEHAVIOUR:
122 |             return None
123 |         elif behaviour_type == py_trees_msgs.Behaviour.SEQUENCE:
124 |             return '#ff9900'
125 |         elif behaviour_type == py_trees_msgs.Behaviour.SELECTOR:
126 |             return '#808080'
127 |         elif behaviour_type == py_trees_msgs.Behaviour.PARALLEL:
128 |             return '#ffd700'
129 |         elif behaviour_type == py_trees_msgs.Behaviour.CHOOSER:
130 |             return '#808080'
131 |         else:
132 |             return None
133 | 
134 |     def type_to_string(self, behaviour_type):
135 |         if behaviour_type == py_trees_msgs.Behaviour.BEHAVIOUR:
136 |             return 'Behaviour'
137 |         elif behaviour_type == py_trees_msgs.Behaviour.SEQUENCE:
138 |             return 'Sequence'
139 |         elif behaviour_type == py_trees_msgs.Behaviour.SELECTOR:
140 |             return 'Selector'
141 |         elif behaviour_type == py_trees_msgs.Behaviour.PARALLEL:
142 |             return 'Parallel'
143 |         elif behaviour_type == py_trees_msgs.Behaviour.CHOOSER:
144 |             return 'Chooser'
145 |         else:
146 |             return None
147 | 
148 |     def status_to_string(self, behaviour_status):
149 |         if behaviour_status == py_trees_msgs.Behaviour.INVALID:
150 |             return 'Invalid'
151 |         elif behaviour_status == py_trees_msgs.Behaviour.RUNNING:
152 |             return 'Running'
153 |         elif behaviour_status == py_trees_msgs.Behaviour.FAILURE:
154 |             return 'Failure'
155 |         elif behaviour_status == py_trees_msgs.Behaviour.SUCCESS:
156 |             return 'Success'
157 |         else:
158 |             return None
159 | 
160 |     def behaviour_to_tooltip_string(self, behaviour):
161 |         to_display = ['class_name', 'type', 'status', 'message']  # should be static
162 |         string = ''
163 | 
164 |         for attr in to_display:
165 |             if attr == 'type':
166 |                 value = self.type_to_string(getattr(behaviour, attr))
167 |             elif attr == 'status':
168 |                 value = self.status_to_string(getattr(behaviour, attr))
169 |             else:
170 |                 value = str(getattr(behaviour, attr))
171 | 
172 |             value = "<i>empty</i>" if not value else value
173 | 
174 |             string += '<b>' + attr.replace('_', ' ').title() + ':</b> ' + value + "<br>"
175 | 
176 |         return "\"" + string + "\""
177 | 
178 |     def generate(self, data, timestamp):
179 |         """
180 |         :param py_trees_msgs.Behaviour[] data:
181 |         :param ??? timestamp:
182 |         """
183 |         graph = self.dotcode_factory.get_graph(rank=self.rank,
184 |                                                rankdir=self.rankdir,
185 |                                                ranksep=self.ranksep)
186 | 
187 |         if len(data) == 0:
188 |             self.dotcode_factory.add_node_to_graph(graph, 'No behaviour data received')
189 |             return graph
190 | 
191 |         # DJS - not actually used? In python3 this is a propblem since
192 |         # unique_id.UniqueID is not hashable any longer
193 |         # behaviour_dict_by_id = {}
194 |         # for behaviour in data:
195 |         #     behaviour_dict_by_id[behaviour.own_id] = behaviour
196 | 
197 |         # first, add nodes to the graph, along with some cached information to
198 |         # make it easy to create the coloured edges on the second pass
199 |         states = {}
200 |         for behaviour in data:
201 |             self.dotcode_factory.add_node_to_graph(graph,
202 |                                                    str(behaviour.own_id),
203 |                                                    nodelabel=behaviour.name,
204 |                                                    shape=self.type_to_shape(behaviour.type),
205 |                                                    color=self.active_and_status_colour_hex_map[(behaviour.is_active, behaviour.status)],
206 |                                                    tooltip=self.behaviour_to_tooltip_string(behaviour))
207 |             states[unique_id.fromMsg(behaviour.own_id)] = (behaviour.is_active, behaviour.status)
208 | 
209 |         for behaviour in data:
210 |             for child_id in behaviour.child_ids:
211 |                 # edge colour is set using integer tuples, not hexes
212 |                 try:
213 |                     (is_active, status) = states[unique_id.fromMsg(child_id)]
214 |                 except KeyError:
215 |                     # the child isn't part of the 'visible' tree
216 |                     continue
217 |                 edge_colour = self.active_and_status_colour_tuple_map[(is_active, status)]
218 |                 self.dotcode_factory.add_edge_to_graph(
219 |                     graph=graph,
220 |                     nodename1=str(behaviour.own_id),
221 |                     nodename2=str(child_id),
222 |                     color=edge_colour
223 |                 )
224 | 
225 |         return graph
226 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/node_item.py:
--------------------------------------------------------------------------------
  1 | #
  2 | # License: Yujin
  3 | #
  4 | ##############################################################################
  5 | # Description
  6 | ##############################################################################
  7 | 
  8 | """
  9 | .. module:: node_item
 10 |    :platform: Unix
 11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.node_item module.
 12 | 
 13 | Oh my spaghettified magnificence,
 14 | Bless my noggin with a tickle from your noodly appendages!
 15 | 
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | from python_qt_binding.QtCore import QPointF, Qt
 23 | from python_qt_binding.QtGui import QBrush, QPolygonF, QPainterPath, QPen
 24 | try:  # indigo
 25 |     from python_qt_binding.QtGui import QGraphicsEllipseItem, QGraphicsPolygonItem, QGraphicsRectItem, QGraphicsSimpleTextItem
 26 | except ImportError:  # kinetic+ (pyqt5)
 27 |     from python_qt_binding.QtWidgets import QGraphicsEllipseItem, QGraphicsPolygonItem, QGraphicsRectItem, QGraphicsSimpleTextItem
 28 | 
 29 | from .graph_item import GraphItem
 30 | 
 31 | ##############################################################################
 32 | # Classes
 33 | ##############################################################################
 34 | 
 35 | 
 36 | class NodeItem(GraphItem):
 37 | 
 38 |     def __init__(self, highlight_level, bounding_box, label, shape, color=None, parent=None, label_pos=None, tooltip=None):
 39 |         super(NodeItem, self).__init__(highlight_level, parent)
 40 | 
 41 |         self._default_color = self._COLOR_BLACK if color is None else color
 42 |         self._brush = QBrush(self._default_color)
 43 |         self._label_pen = QPen()
 44 |         self._label_pen.setColor(self._default_color)
 45 |         self._label_pen.setJoinStyle(Qt.RoundJoin)
 46 |         self._ellipse_pen = QPen(self._label_pen)
 47 |         self._ellipse_pen.setWidth(1)
 48 | 
 49 |         self._incoming_edges = set()
 50 |         self._outgoing_edges = set()
 51 | 
 52 |         if shape == 'box':
 53 |             self._graphics_item = QGraphicsRectItem(bounding_box)
 54 | 
 55 |         # Since we don't have unique GraphicsItems other than Ellipse and Rect,
 56 |         # Using Polygon to draw the following using bounding_box
 57 | 
 58 |         elif shape == 'octagon':
 59 |             rect = bounding_box.getRect()
 60 |             octagon_polygon = QPolygonF([QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
 61 |                                          QPointF(rect[0], rect[1] + 7 * rect[3] / 10),
 62 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3]),
 63 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3]),
 64 |                                          QPointF(rect[0] + rect[2], rect[1] + 7 * rect[3] / 10),
 65 |                                          QPointF(rect[0] + rect[2], rect[1] + 3 * rect[3] / 10),
 66 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1]),
 67 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1])])
 68 |             self._graphics_item = QGraphicsPolygonItem(octagon_polygon)
 69 | 
 70 |         elif shape == 'doubleoctagon':
 71 |             rect = bounding_box.getRect()
 72 |             inner_fold = 3.0
 73 | 
 74 |             octagon_polygon = QPolygonF([QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
 75 |                                          QPointF(rect[0], rect[1] + 7 * rect[3] / 10),
 76 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3]),
 77 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3]),
 78 |                                          QPointF(rect[0] + rect[2], rect[1] + 7 * rect[3] / 10),
 79 |                                          QPointF(rect[0] + rect[2], rect[1] + 3 * rect[3] / 10),
 80 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1]),
 81 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1]),
 82 |                                          # inner
 83 |                                          QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
 84 |                                          QPointF(rect[0] + inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2),
 85 |                                          QPointF(rect[0] + inner_fold, rect[1] + 7 * rect[3] / 10 - inner_fold / 2),
 86 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3] - inner_fold),
 87 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3] - inner_fold),
 88 |                                          QPointF(rect[0] + rect[2] - inner_fold, rect[1] + 7 * rect[3] / 10 - inner_fold / 2),
 89 |                                          QPointF(rect[0] + rect[2] - inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2),
 90 |                                          QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + inner_fold),
 91 |                                          QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + inner_fold),
 92 |                                          QPointF(rect[0] + inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2)
 93 |                                          ])
 94 | 
 95 |             self._graphics_item = QGraphicsPolygonItem(octagon_polygon)
 96 | 
 97 |         elif shape == 'note':
 98 |             rect = bounding_box.getRect()
 99 |             note_polygon = QPolygonF([QPointF(rect[0] + 9 * rect[2] / 10, rect[1]),
100 |                                       QPointF(rect[0], rect[1]),
101 |                                       QPointF(rect[0], rect[1] + rect[3]),
102 |                                       QPointF(rect[0] + rect[2], rect[1] + rect[3]),
103 |                                       QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5),
104 |                                       QPointF(rect[0] + 9 * rect[2] / 10, rect[1] + rect[3] / 5),
105 |                                       QPointF(rect[0] + 9 * rect[2] / 10, rect[1]),
106 |                                       QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5),
107 |                                       QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5)])
108 |             self._graphics_item = QGraphicsPolygonItem(note_polygon)
109 | 
110 |         else:
111 |             self._graphics_item = QGraphicsEllipseItem(bounding_box)
112 |         self.addToGroup(self._graphics_item)
113 | 
114 |         self._label = QGraphicsSimpleTextItem(label)
115 |         label_rect = self._label.boundingRect()
116 |         if label_pos is None:
117 |             label_rect.moveCenter(bounding_box.center())
118 |         else:
119 |             label_rect.moveCenter(label_pos)
120 |         self._label.setPos(label_rect.x(), label_rect.y())
121 |         self.addToGroup(self._label)
122 |         if tooltip is not None:
123 |             self.setToolTip(tooltip)
124 | 
125 |         self.set_node_color()
126 | 
127 |         self.setAcceptHoverEvents(True)
128 | 
129 |         self.hovershape = None
130 | 
131 |     def set_hovershape(self, newhovershape):
132 |         self.hovershape = newhovershape
133 | 
134 |     def shape(self):
135 |         if self.hovershape is not None:
136 |             path = QPainterPath()
137 |             path.addRect(self.hovershape)
138 |             return path
139 |         else:
140 |             return super(self.__class__, self).shape()
141 | 
142 |     def add_incoming_edge(self, edge):
143 |         self._incoming_edges.add(edge)
144 | 
145 |     def add_outgoing_edge(self, edge):
146 |         self._outgoing_edges.add(edge)
147 | 
148 |     def set_node_color(self, color=None):
149 |         if color is None:
150 |             color = self._default_color
151 | 
152 |         self._brush.setColor(color)
153 |         self._ellipse_pen.setColor(color)
154 |         self._label_pen.setColor(color)
155 | 
156 |         self._graphics_item.setPen(self._ellipse_pen)
157 |         self._label.setBrush(self._brush)
158 |         self._label.setPen(self._label_pen)
159 | 
160 |     def hoverEnterEvent(self, event):
161 |         # hovered node item in red
162 |         self.set_node_color(self._COLOR_RED)
163 | 
164 |         if self._highlight_level > 1:
165 |             cyclic_edges = self._incoming_edges.intersection(self._outgoing_edges)
166 |             # incoming edges in blue
167 |             incoming_nodes = set()
168 |             for incoming_edge in self._incoming_edges.difference(cyclic_edges):
169 |                 incoming_edge.set_node_color(self._COLOR_BLUE)
170 |                 if incoming_edge.from_node != self:
171 |                     incoming_nodes.add(incoming_edge.from_node)
172 |             # outgoing edges in green
173 |             outgoing_nodes = set()
174 |             for outgoing_edge in self._outgoing_edges.difference(cyclic_edges):
175 |                 outgoing_edge.set_node_color(self._COLOR_GREEN)
176 |                 if outgoing_edge.to_node != self:
177 |                     outgoing_nodes.add(outgoing_edge.to_node)
178 |             # incoming/outgoing edges in teal
179 |             for edge in cyclic_edges:
180 |                 edge.set_node_color(self._COLOR_TEAL)
181 | 
182 |             if self._highlight_level > 2:
183 |                 cyclic_nodes = incoming_nodes.intersection(outgoing_nodes)
184 |                 # incoming nodes in blue
185 |                 for incoming_node in incoming_nodes.difference(cyclic_nodes):
186 |                     incoming_node.set_node_color(self._COLOR_BLUE)
187 |                 # outgoing nodes in green
188 |                 for outgoing_node in outgoing_nodes.difference(cyclic_nodes):
189 |                     outgoing_node.set_node_color(self._COLOR_GREEN)
190 |                 # incoming/outgoing nodes in teal
191 |                 for node in cyclic_nodes:
192 |                     node.set_node_color(self._COLOR_TEAL)
193 | 
194 |     def hoverLeaveEvent(self, event):
195 |         self.set_node_color()
196 |         if self._highlight_level > 1:
197 |             for incoming_edge in self._incoming_edges:
198 |                 incoming_edge.set_node_color()
199 |                 if self._highlight_level > 2 and incoming_edge.from_node != self:
200 |                     incoming_edge.from_node.set_node_color()
201 |             for outgoing_edge in self._outgoing_edges:
202 |                 outgoing_edge.set_node_color()
203 |                 if self._highlight_level > 2 and outgoing_edge.to_node != self:
204 |                     outgoing_edge.to_node.set_node_color()
205 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/qt_dotgraph/dot_to_qt.py:
--------------------------------------------------------------------------------
  1 | #
  2 | # License: Yujin
  3 | #
  4 | ##############################################################################
  5 | # Description
  6 | ##############################################################################
  7 | 
  8 | """
  9 | .. module:: dot_to_qt
 10 |    :platform: Unix
 11 |    :synopsis: Repackaging of the limiting ROS qt_dotgraph.dot_to_qt module.
 12 | 
 13 | Oh my spaghettified magnificence,
 14 | Bless my noggin with a tickle from your noodly appendages!
 15 | 
 16 | """
 17 | 
 18 | ##############################################################################
 19 | # Imports
 20 | ##############################################################################
 21 | 
 22 | import codecs
 23 | 
 24 | # import pydot
 25 | import pygraphviz
 26 | import pyparsing
 27 | 
 28 | # work around for https://bugs.launchpad.net/ubuntu/+source/pydot/+bug/1321135
 29 | pyparsing._noncomma = "".join([c for c in pyparsing.printables if c != ","])
 30 | 
 31 | 
 32 | from python_qt_binding.QtCore import QPointF, QRectF
 33 | from python_qt_binding.QtGui import QColor
 34 | 
 35 | from .edge_item import EdgeItem
 36 | from .node_item import NodeItem
 37 | 
 38 | 
 39 | POINTS_PER_INCH = 72
 40 | 
 41 | ##############################################################################
 42 | # Support
 43 | ##############################################################################
 44 | 
 45 | 
 46 | # hack required by pydot
 47 | def get_unquoted(item, name):
 48 |     value = item.get(name)
 49 |     if value is None:
 50 |         return None
 51 |     try:
 52 |         return value.strip('"\n"')
 53 |     except AttributeError:
 54 |         # not part of the string family
 55 |         return value
 56 | 
 57 | # approximately, for workarounds (TODO: get this from dotfile somehow)
 58 | LABEL_HEIGHT = 30
 59 | 
 60 | ##############################################################################
 61 | # Classes
 62 | ##############################################################################
 63 | 
 64 | 
 65 | # Class generating Qt Elements from doctcode
 66 | class DotToQtGenerator():
 67 | 
 68 |     def __init__(self):
 69 |         pass
 70 | 
 71 |     def getNodeItemForSubgraph(self, subgraph, highlight_level):
 72 |         # let pydot imitate pygraphviz api
 73 |         attr = {}
 74 |         for name in subgraph.get_attributes().iterkeys():
 75 |             value = get_unquoted(subgraph, name)
 76 |             attr[name] = value
 77 |         obj_dic = subgraph.__getattribute__("obj_dict")
 78 |         for name in obj_dic:
 79 |             if name not in ['nodes', 'attributes', 'parent_graph'] and obj_dic[name] is not None:
 80 |                 attr[name] = get_unquoted(obj_dic, name)
 81 |             elif name == 'nodes':
 82 |                 for key in obj_dic['nodes']['graph'][0]['attributes']:
 83 |                     attr[key] = get_unquoted(obj_dic['nodes']['graph'][0]['attributes'], key)
 84 |         subgraph.attr = attr
 85 | 
 86 |         bb = subgraph.attr.get('bb', None)
 87 |         if bb is None:
 88 |             # no bounding box
 89 |             return None
 90 |         bb = bb.strip('"').split(',')
 91 |         if len(bb) < 4:
 92 |             # bounding box is empty
 93 |             return None
 94 |         bounding_box = QRectF(0, 0, float(bb[2]) - float(bb[0]), float(bb[3]) - float(bb[1]))
 95 |         if 'lp' in subgraph.attr:
 96 |             label_pos = subgraph.attr['lp'].strip('"').split(',')
 97 |         else:
 98 |             label_pos = (float(bb[0]) + (float(bb[2]) - float(bb[0])) / 2, float(bb[1]) + (float(bb[3]) - float(bb[1])) - LABEL_HEIGHT / 2)
 99 |         bounding_box.moveCenter(QPointF(float(bb[0]) + (float(bb[2]) - float(bb[0])) / 2, -float(bb[1]) - (float(bb[3]) - float(bb[1])) / 2))
100 |         name = subgraph.attr.get('label', '')
101 |         color = QColor(subgraph.attr['color']) if 'color' in subgraph.attr else None
102 |         subgraph_nodeitem = NodeItem(highlight_level,
103 |                                      bounding_box,
104 |                                      label=name,
105 |                                      shape='box',
106 |                                      color=color,
107 |                                      label_pos=QPointF(float(label_pos[0]), -float(label_pos[1])))
108 |         bounding_box = QRectF(bounding_box)
109 |         # With clusters we have the problem that mouse hovers cannot
110 |         # decide whether to be over the cluster or a subnode. Using
111 |         # just the "title area" solves this. TODO: Maybe using a
112 |         # border region would be even better (multiple RectF)
113 |         bounding_box.setHeight(LABEL_HEIGHT)
114 |         subgraph_nodeitem.set_hovershape(bounding_box)
115 |         return subgraph_nodeitem
116 | 
117 |     def getNodeItemForNode(self, node, highlight_level):
118 |         """
119 |         returns a pyqt NodeItem object, or None in case of error or invisible style
120 |         """
121 |         # let pydot imitate pygraphviz api
122 | #         attr = {}
123 | #         for name in node.get_attributes().iterkeys():
124 | #             value = get_unquoted(node, name)
125 | #             attr[name] = value
126 | #         obj_dic = node.__getattribute__("obj_dict")
127 | #         for name in obj_dic:
128 | #             if name not in ['attributes', 'parent_graph'] and obj_dic[name] is not None:
129 | #                 attr[name] = get_unquoted(obj_dic, name)
130 | #         node.attr = attr
131 | 
132 |         if 'style' in node.attr:
133 |             if node.attr['style'] == 'invis':
134 |                 return None
135 | 
136 |         color = QColor(node.attr['color']) if 'color' in node.attr else None
137 |         name = None
138 |         if 'label' in node.attr:
139 |             name = node.attr['label']
140 |         elif 'name' in node.attr:
141 |             name = node.attr['name']
142 |         else:
143 |             print("Error, no label defined for node with attr: %s" % node.attr)
144 |             return None
145 |         if name is None:
146 |             # happens on Lucid pygraphviz version
147 |             print("Error, label is None for node %s, pygraphviz version may be too old." % node)
148 |         else:
149 |             # python2
150 |             # name = name.decode('string_escape')
151 |             # python3
152 |             name = codecs.escape_decode(name)[0].decode('utf-8')
153 | 
154 |         # decrease rect by one so that edges do not reach inside
155 |         bb_width = len(name) / 5
156 |         if 'width' in node.attr:
157 |             bb_width = node.attr['width']
158 |         bb_height = 1.0
159 |         if 'width' in node.attr:
160 |             bb_height = node.attr['height']
161 |         bounding_box = QRectF(0, 0, POINTS_PER_INCH * float(bb_width) - 1.0, POINTS_PER_INCH * float(bb_height) - 1.0)
162 |         pos = (0, 0)
163 |         if 'pos' in node.attr:
164 |             pos = node.attr['pos'].split(',')
165 |         bounding_box.moveCenter(QPointF(float(pos[0]), -float(pos[1])))
166 | 
167 |         node_item = NodeItem(highlight_level=highlight_level,
168 |                              bounding_box=bounding_box,
169 |                              label=name,
170 |                              shape=node.attr.get('shape', 'ellipse'),
171 |                              color=color,
172 |                              tooltip=node.attr.get('tooltip', None)
173 |                              # parent=None,
174 |                              # label_pos=None
175 |                              )
176 |         # node_item.setToolTip(self._generate_tool_tip(node.attr.get('URL', None)))
177 |         return node_item
178 | 
179 |     def addEdgeItem(self, edge, nodes, edges, highlight_level, same_label_siblings=False):
180 |         """
181 |         adds EdgeItem by data in edge to edges
182 |         :param same_label_siblings: if true, edges with same label will be considered siblings (collective highlighting)
183 |         """
184 |         # let pydot imitate pygraphviz api
185 | #         attr = {}
186 | #         for name in edge.get_attributes().iterkeys():
187 | #             value = get_unquoted(edge, name)
188 | #             attr[name] = value
189 | #         edge.attr = attr
190 | 
191 |         if 'style' in edge.attr:
192 |             if edge.attr['style'] == 'invis':
193 |                 return
194 |         style = edge.attr.get('style', None)
195 | 
196 |         label = edge.attr.get('label', None)
197 |         label_pos = edge.attr.get('lp', None)
198 |         label_center = None
199 |         if label_pos is not None:
200 |             label_pos = label_pos.split(',')
201 |             label_center = QPointF(float(label_pos[0]), -float(label_pos[1]))
202 | 
203 |         # try pydot, fallback for pygraphviz
204 |         source_node = edge.get_source() if hasattr(edge, 'get_source') else edge[0]
205 |         destination_node = edge.get_destination() if hasattr(edge, 'get_destination') else edge[1]
206 | 
207 |         # create edge with from-node and to-node
208 |         edge_pos = "0,0"
209 |         if 'pos' in edge.attr:
210 |             edge_pos = edge.attr['pos']
211 |         if label is not None:
212 |             label = label.decode('string_escape')
213 | 
214 |         color = None
215 |         if 'colorR' in edge.attr and 'colorG' in edge.attr and 'colorB' in edge.attr:
216 |             r = edge.attr['colorR']
217 |             g = edge.attr['colorG']
218 |             b = edge.attr['colorB']
219 |             color = QColor(float(r), float(g), float(b))
220 | 
221 |         edge_item = EdgeItem(highlight_level=highlight_level,
222 |                              spline=edge_pos,
223 |                              label_center=label_center,
224 |                              label=label,
225 |                              from_node=nodes[source_node],
226 |                              to_node=nodes[destination_node],
227 |                              penwidth=int(edge.attr['penwidth']),
228 |                              edge_color=color,
229 |                              style=style)
230 | 
231 |         if same_label_siblings:
232 |             if label is None:
233 |                 # for sibling detection
234 |                 label = "%s_%s" % (source_node, destination_node)
235 |             # symmetrically add all sibling edges with same label
236 |             if label in edges:
237 |                 for sibling in edges[label]:
238 |                     edge_item.add_sibling_edge(sibling)
239 |                     sibling.add_sibling_edge(edge_item)
240 | 
241 |         if label not in edges:
242 |             edges[label] = []
243 |         edges[label].append(edge_item)
244 | 
245 |     def dotcode_to_qt_items(self, dotcode, highlight_level, same_label_siblings=False):
246 |         """
247 |         takes dotcode, runs layout, and creates qt items based on the dot layout.
248 |         returns two dicts, one mapping node names to Node_Item, one mapping edge names to lists of Edge_Item
249 |         :param same_label_siblings: if true, edges with same label will be considered siblings (collective highlighting)
250 |         """
251 |         # layout graph
252 |         if dotcode is None:
253 |             return {}, {}
254 | 
255 |         # pydot - this function is very buggy and expensive, quickly > 1s!
256 |         # graph = pydot.graph_from_dot_data(dotcode.encode("ascii", "ignore"))
257 |         # let pydot imitate pygraphviz api
258 |         # graph.nodes_iter = graph.get_node_list
259 |         # graph.edges_iter = graph.get_edge_list
260 |         # graph.subgraphs_iter = graph.get_subgraph_list
261 | 
262 |         # pygraphviz
263 |         graph = pygraphviz.AGraph(
264 |             string=dotcode,  # .encode("ascii", "ignore"),
265 |             strict=False,
266 |             directed=True
267 |         )
268 |         graph.layout(prog='dot')
269 | 
270 |         nodes = {}
271 |         for subgraph in graph.subgraphs_iter():
272 |             subgraph_nodeitem = self.getNodeItemForSubgraph(subgraph, highlight_level)
273 |             # skip subgraphs with empty bounding boxes
274 |             if subgraph_nodeitem is None:
275 |                 continue
276 | 
277 |             subgraph.nodes_iter = subgraph.get_node_list
278 |             nodes[subgraph.get_name()] = subgraph_nodeitem
279 |             for node in subgraph.nodes_iter():
280 |                 # hack required by pydot
281 |                 if node.get_name() in ('graph', 'node', 'empty'):
282 |                     continue
283 |                 nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level)
284 |         for node in graph.nodes_iter():
285 |             # hack required by pydot
286 |             if node.get_name() in ('graph', 'node', 'empty'):
287 |                 continue
288 |             nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level)
289 | 
290 |         edges = {}
291 | 
292 |         for subgraph in graph.subgraphs_iter():
293 |             subgraph.edges_iter = subgraph.get_edge_list
294 |             for edge in subgraph.edges_iter():
295 |                 self.addEdgeItem(edge, nodes, edges,
296 |                                  highlight_level=highlight_level,
297 |                                  same_label_siblings=same_label_siblings)
298 | 
299 |         for edge in graph.edges_iter():
300 |             self.addEdgeItem(edge, nodes, edges,
301 |                              highlight_level=highlight_level,
302 |                              same_label_siblings=same_label_siblings)
303 | 
304 |         return nodes, edges
305 | 
306 |     def graph_to_qt_items(self, graph, highlight_level, same_label_siblings=False):
307 |         """
308 |         takes a pydot/pygraphviz graph and creates qt items based on the dot layout.
309 |         returns two dicts, one mapping node names to Node_Item, one mapping edge names to lists of Edge_Item
310 |         :param same_label_siblings: if true, edges with same label will be considered siblings (collective highlighting)
311 |         """
312 |         if graph is None:
313 |             return {}, {}
314 | 
315 |         # if pydot, let pydot imitate pygraphviz api
316 |         # graph.nodes_iter = graph.get_node_list
317 |         # graph.edges_iter = graph.get_edge_list
318 |         # graph.subgraphs_iter = graph.get_subgraph_list
319 | 
320 |         nodes = {}
321 |         for subgraph in graph.subgraphs_iter():
322 |             subgraph_nodeitem = self.getNodeItemForSubgraph(subgraph, highlight_level)
323 |             # skip subgraphs with empty bounding boxes
324 |             if subgraph_nodeitem is None:
325 |                 continue
326 | 
327 |             subgraph.nodes_iter = subgraph.get_node_list
328 |             nodes[subgraph.get_name()] = subgraph_nodeitem
329 |             for node in subgraph.nodes_iter():
330 |                 # hack required by pydot
331 |                 if node.get_name() in ('graph', 'node', 'empty'):
332 |                     continue
333 |                 nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level)
334 |         for node in graph.nodes_iter():
335 |             # hack required by pydot
336 |             if node.get_name() in ('graph', 'node', 'empty'):
337 |                 continue
338 |             nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level)
339 | 
340 |         edges = {}
341 | 
342 |         for subgraph in graph.subgraphs_iter():
343 |             subgraph.edges_iter = subgraph.get_edge_list
344 |             for edge in subgraph.edges_iter():
345 |                 self.addEdgeItem(edge, nodes, edges,
346 |                                  highlight_level=highlight_level,
347 |                                  same_label_siblings=same_label_siblings)
348 | 
349 |         for edge in graph.edges_iter():
350 |             self.addEdgeItem(edge, nodes, edges,
351 |                              highlight_level=highlight_level,
352 |                              same_label_siblings=same_label_siblings)
353 | 
354 |         return nodes, edges
355 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/dynamic_timeline.py:
--------------------------------------------------------------------------------
  1 | # Software License Agreement (BSD License)
  2 | #
  3 | # Copyright (c) 2012, Willow Garage, Inc.
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions
  8 | # are met:
  9 | #
 10 | #  * Redistributions of source code must retain the above copyright
 11 | #    notice, this list of conditions and the following disclaimer.
 12 | #  * Redistributions in binary form must reproduce the above
 13 | #    copyright notice, this list of conditions and the following
 14 | #    disclaimer in the documentation and/or other materials provided
 15 | #    with the distribution.
 16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
 17 | #    contributors may be used to endorse or promote products derived
 18 | #    from this software without specific prior written permission.
 19 | #
 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 31 | # POSSIBILITY OF SUCH DAMAGE.
 32 | 
 33 | import rospy
 34 | import time
 35 | import threading
 36 | import collections
 37 | import itertools
 38 | import bisect
 39 | 
 40 | from python_qt_binding.QtCore import Qt, QTimer, qWarning, Signal
 41 | try:  # indigo
 42 |     from python_qt_binding.QtGui import QGraphicsScene, QMessageBox
 43 | except ImportError:  # kinetic+ (pyqt5)
 44 |     from python_qt_binding.QtWidgets import QGraphicsScene, QMessageBox
 45 | 
 46 | from .dynamic_timeline_frame import DynamicTimelineFrame
 47 | from rqt_bag.message_listener_thread import MessageListenerThread
 48 | from .message_loader_thread import MessageLoaderThread
 49 | from rqt_bag.player import Player
 50 | from rqt_bag.recorder import Recorder
 51 | from rqt_bag.timeline_menu import TimelinePopupMenu
 52 | 
 53 | from . import topic_helper
 54 | 
 55 | 
 56 | class DynamicTimeline(QGraphicsScene):
 57 |     """
 58 |     BagTimeline contains bag files, all information required to display the bag data visualization on the screen
 59 |     Also handles events
 60 |     """
 61 |     status_bar_changed_signal = Signal()
 62 |     selected_region_changed = Signal(rospy.Time, rospy.Time)
 63 |     timeline_updated = Signal()
 64 |     Topic = collections.namedtuple('Topic', ['subscriber', 'queue'])
 65 |     Message = collections.namedtuple('Message', ['stamp', 'message'])
 66 | 
 67 |     def __init__(self, context, publish_clock):
 68 |         """
 69 |         :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
 70 |         """
 71 |         super(DynamicTimeline, self).__init__()
 72 |         # key is topic name, value is a named tuple of type Topic. The deque
 73 |         # contains named tuples of type Message
 74 |         self._topics = {}
 75 |         # key is the data type, value is a list of topics with that type
 76 |         self._datatypes = {}
 77 |         self._topic_lock = threading.RLock()
 78 | 
 79 |         self.background_task = None  # Display string
 80 |         self.background_task_cancel = False
 81 | 
 82 |         # Playing / Recording
 83 |         self._playhead_lock = threading.RLock()
 84 |         self._max_play_speed = 1024.0  # fastest X play speed
 85 |         self._min_play_speed = 1.0 / 1024.0  # slowest X play speed
 86 |         self._play_speed = 0.0
 87 |         self._play_all = False
 88 |         self._playhead_positions_cvs = {}
 89 |         self._playhead_positions = {}  # topic -> position
 90 |         self._message_loaders = {}
 91 |         self._messages_cvs = {}
 92 |         self._messages = {}  # topic -> msg_data
 93 |         self._message_listener_threads = {}  # listener -> MessageListenerThread
 94 |         self._player = False
 95 |         self._publish_clock = publish_clock
 96 |         self._recorder = None
 97 |         self.last_frame = None
 98 |         self.last_playhead = None
 99 |         self.desired_playhead = None
100 |         self.wrap = True  # should the playhead wrap when it reaches the end?
101 |         self.stick_to_end = False  # should the playhead stick to the end?
102 |         self._play_timer = QTimer()
103 |         self._play_timer.timeout.connect(self.on_idle)
104 |         self._play_timer.setInterval(3)
105 |         self._redraw_timer = None  # timer which can be used to periodically redraw the timeline
106 | 
107 |         # Plugin popup management
108 |         self._context = context
109 |         self.popups = {}
110 |         self._views = []
111 |         self._listeners = {}
112 | 
113 |         # Initialize scene
114 |         # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
115 |         # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
116 |         self.setBackgroundBrush(Qt.white)
117 |         self._timeline_frame = DynamicTimelineFrame(self)
118 |         self._timeline_frame.setPos(0, 0)
119 |         self.addItem(self._timeline_frame)
120 | 
121 |         self.background_progress = 0
122 |         self.__closed = False
123 | 
124 |         # timer to periodically redraw the timeline every so often
125 |         self._start_redraw_timer()
126 | 
127 |     def get_context(self):
128 |         """
129 |         :returns: the ROS_GUI context, 'PluginContext'
130 |         """
131 |         return self._context
132 | 
133 |     def _start_redraw_timer(self):
134 |         if not self._redraw_timer:
135 |             self._redraw_timer = rospy.Timer(rospy.Duration(0.5), self._redraw_timeline)
136 | 
137 |     def _stop_redraw_timer(self):
138 |         if self._redraw_timer:
139 |             self._redraw_timer.shutdown()
140 |             self._redraw_timer = None
141 | 
142 |     def handle_close(self):
143 |         """
144 |         Cleans up the timeline, subscribers and any threads
145 |         """
146 |         if self.__closed:
147 |             return
148 |         else:
149 |             self.__closed = True
150 |         self._play_timer.stop()
151 |         for topic in self._get_topics():
152 |             self.stop_publishing(topic)
153 |             self._message_loaders[topic].stop()
154 |         if self._player:
155 |             self._player.stop()
156 |         if self._recorder:
157 |             self._recorder.stop()
158 |         if self.background_task is not None:
159 |             self.background_task_cancel = True
160 |         self._timeline_frame.handle_close()
161 |         for topic in self._topics:
162 |             self._topics[topic][0].unregister()  # unregister the subscriber
163 |         for frame in self._views:
164 |             if frame.parent():
165 |                 self._context.remove_widget(frame)
166 | 
167 |     def _redraw_timeline(self, timer):
168 |         # save the playhead so that the redraw doesn't move it
169 |         playhead = self._timeline_frame._playhead
170 |         if playhead is None:
171 |             start = self._timeline_frame.play_region[0] is None
172 |             end = self._timeline_frame.play_region[1] is None
173 |         else:
174 |             start = True if playhead <= self._timeline_frame.play_region[0] else False
175 |             end = True if playhead >= self._timeline_frame.play_region[1] else False
176 | 
177 |         # do not keep setting this if you want the timeline to just grow.
178 |         self._timeline_frame._start_stamp = self._get_start_stamp()
179 |         self._timeline_frame._end_stamp = self._get_end_stamp()
180 | 
181 |         self._timeline_frame.reset_zoom()
182 | 
183 |         if end:
184 |             # use play region instead of time.now to stop playhead going past
185 |             # the end of the region, which causes problems with
186 |             # navigate_previous
187 |             self._timeline_frame._set_playhead(self._timeline_frame.play_region[1])
188 |         elif start:
189 |             self._timeline_frame._set_playhead(self._timeline_frame.play_region[0])
190 |         else:
191 |             if playhead:
192 |                 self._timeline_frame._set_playhead(playhead)
193 | 
194 |         self.timeline_updated.emit()
195 | 
196 |     def topic_callback(self, msg, topic):
197 |         """
198 |         Called whenever a message is received on any of the subscribed topics
199 | 
200 |         :param topic: the topic on which the message was received
201 |         :param msg: the message received
202 | 
203 |         """
204 |         message = self.Message(stamp=rospy.Time.now(), message=msg)
205 |         with self._topic_lock:
206 |             self._topics[topic].queue.append(message)
207 | 
208 |         # Invalidate entire cache for this topic
209 |         with self._timeline_frame.index_cache_cv:
210 |             self._timeline_frame.invalidated_caches.add(topic)
211 |             # if topic in self._timeline_frame.index_cache:
212 |             #     del self._timeline_frame.index_cache[topic]
213 |             self._timeline_frame.index_cache_cv.notify()
214 | 
215 |     def add_topic(self, topic, type, num_msgs=20):
216 |         """Creates an indexing thread for the new topic. Fixes the borders and notifies
217 |         the indexing thread to index the new items bags
218 | 
219 |         :param topic: a topic to listen to
220 |         :param type: type of the topic to listen to
221 |         :param num_msgs: number of messages to retain on this topic. If this
222 |             value is exceeded, the oldest messages will be dropped
223 | 
224 |         :return: false if topic already in the timeline, true otherwise
225 | 
226 |         """
227 |         # first item in each sub-list is the name, second is type
228 |         if topic not in self._topics:
229 |             self._topics[topic] = self.Topic(subscriber=rospy.Subscriber(topic, type, queue_size=1, callback=self.topic_callback, callback_args=topic),
230 |                                              queue=collections.deque(maxlen=num_msgs))
231 |             self._datatypes.setdefault(type, []).append(topic)
232 |         else:
233 |             return False
234 | 
235 |         self._playhead_positions_cvs[topic] = threading.Condition()
236 |         self._messages_cvs[topic] = threading.Condition()
237 |         self._message_loaders[topic] = MessageLoaderThread(self, topic)
238 | 
239 |         self._timeline_frame._start_stamp = self._get_start_stamp()
240 |         self._timeline_frame._end_stamp = self._get_end_stamp()
241 |         self._timeline_frame.topics = self._get_topics()
242 |         self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype()
243 |         # If this is the first bag, reset the timeline
244 |         self._timeline_frame.reset_zoom()
245 | 
246 |         # Invalidate entire cache for this topic
247 |         with self._timeline_frame.index_cache_cv:
248 |             self._timeline_frame.invalidated_caches.add(topic)
249 |             if topic in self._timeline_frame.index_cache:
250 |                 del self._timeline_frame.index_cache[topic]
251 |             self._timeline_frame.index_cache_cv.notify()
252 | 
253 |         return True
254 | 
255 |     # TODO Rethink API and if these need to be visible
256 |     def _get_start_stamp(self):
257 |         """
258 | 
259 |         :return: stamp of the first message received on any of the topics, or none if no messages have been received, ''rospy.Time''
260 |         """
261 |         with self._topic_lock:
262 |             start_stamp = None
263 |             for unused_topic_name, topic_tuple in self._topics.items():
264 |                 topic_start_stamp = topic_helper.get_start_stamp(topic_tuple)
265 |                 if topic_start_stamp is not None and (start_stamp is None or topic_start_stamp < start_stamp):
266 |                     start_stamp = topic_start_stamp
267 |             return start_stamp
268 | 
269 |     def _get_end_stamp(self):
270 |         """
271 | 
272 |         :return: stamp of the last message received on any of the topics, or none if no messages have been received, ''rospy.Time''
273 |         """
274 |         with self._topic_lock:
275 |             end_stamp = None
276 |             for unused_topic_name, topic_tuple in self._topics.items():
277 |                 topic_end_stamp = topic_helper.get_end_stamp(topic_tuple)
278 |                 if topic_end_stamp is not None and (end_stamp is None or topic_end_stamp > end_stamp):
279 |                     end_stamp = topic_end_stamp
280 |             return end_stamp
281 | 
282 |     def _get_topics(self):
283 |         """
284 |         :return: sorted list of topic names, ''list(str)''
285 |         """
286 |         with self._topic_lock:
287 |             topics = []
288 |             for topic in self._topics:
289 |                 topics.append(topic)
290 |             return sorted(topics)
291 | 
292 |     def _get_topics_by_datatype(self):
293 |         """
294 |         :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
295 |         """
296 |         with self._topic_lock:
297 |             return self._datatypes
298 | 
299 |     def get_datatype(self, topic):
300 |         """
301 |         :return: datatype associated with a topic, ''str''
302 |         :raises: if there are multiple datatypes assigned to a single topic, ''Exception''
303 |         """
304 |         with self._topic_lock:
305 |             topic_types = []
306 |             for datatype in self._datatypes:
307 |                 if topic in self._datatypes[datatype]:
308 |                     if len(topic_types) == 1:
309 |                         raise Exception("Topic {0} had multiple datatypes ({1}) associated with it".format(topic, str(topic_types)))
310 |                     topic_types.append(datatype._type)
311 | 
312 |             if not topic_types:
313 |                 return None
314 |             else:
315 |                 return topic_types[0]
316 | 
317 |     def get_entries(self, topics, start_stamp, end_stamp):
318 |         """
319 |         generator function for topic entries
320 |         :param topics: list of topics to query, ''list(str)''
321 |         :param start_stamp: stamp to start at, ''rospy.Time''
322 |         :param end_stamp: stamp to end at, ''rospy,Time''
323 |         :returns: messages on the given topics in chronological order, ''msg''
324 |         """
325 |         with self._topic_lock:
326 |             topic_entries = []
327 |             # make sure that we can handle a single topic as well
328 |             for topic in topics:
329 |                 if not topic in self._topics:
330 |                     rospy.logwarn("Dynamic Timeline : Topic {0} was not in the topic list. Skipping.".format(topic))
331 |                     continue
332 | 
333 |                 # don't bother with topics if they don't overlap the requested time range
334 |                 topic_start_time = topic_helper.get_start_stamp(self._topics[topic])
335 |                 if topic_start_time is not None and topic_start_time > end_stamp:
336 |                     continue
337 | 
338 |                 topic_end_time = topic_helper.get_end_stamp(self._topics[topic])
339 |                 if topic_end_time is not None and topic_end_time < start_stamp:
340 |                     continue
341 | 
342 |                 topic_queue = self._topics[topic].queue
343 |                 start_ind, first_entry = self._entry_at(start_stamp, topic_queue)
344 |                 # entry returned might be the latest one before the start stamp
345 |                 # if there isn't one exactly on the stamp - if so, start from
346 |                 # the next entry
347 |                 if first_entry.stamp < start_stamp:
348 |                     start_ind += 1
349 | 
350 |                 # entry at always returns entry at or before the given stamp, so
351 |                 # no manipulation needed.
352 |                 end_ind, last_entry = self._entry_at(end_stamp, topic_queue)
353 | 
354 |                 topic_entries.extend(list(itertools.islice(topic_queue, start_ind, end_ind + 1)))
355 | 
356 |             for entry in sorted(topic_entries, key=lambda x: x.stamp):
357 |                 yield entry
358 | 
359 |     def get_entries_with_bags(self, topic, start_stamp, end_stamp):
360 |         """
361 |         generator function for bag entries
362 |         :param topics: list of topics to query, ''list(str)''
363 |         :param start_stamp: stamp to start at, ''rospy.Time''
364 |         :param end_stamp: stamp to end at, ''rospy,Time''
365 |         :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
366 |         """
367 |         with self._bag_lock:
368 |             from rosbag import bag  # for _mergesort
369 | 
370 |             bag_entries = []
371 |             bag_by_iter = {}
372 |             for b in self._bags:
373 |                 bag_start_time = bag_helper.get_start_stamp(b)
374 |                 if bag_start_time is not None and bag_start_time > end_stamp:
375 |                     continue
376 | 
377 |                 bag_end_time = bag_helper.get_end_stamp(b)
378 |                 if bag_end_time is not None and bag_end_time < start_stamp:
379 |                     continue
380 | 
381 |                 connections = list(b._get_connections(topic))
382 |                 it = iter(b._get_entries(connections, start_stamp, end_stamp))
383 |                 bag_by_iter[it] = b
384 |                 bag_entries.append(it)
385 | 
386 |             for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time):
387 |                 yield bag_by_iter[it], entry
388 | 
389 |     def _entry_at(self, t, queue):
390 |         """Get the entry and index in the queue at the given time.
391 | 
392 |         :param ``rospy.Time`` t: time to check
393 |         :param ``collections.deque`` queue: deque to look at
394 | 
395 |         :return: (index, Message) tuple. If there is no message in the queue at
396 |             the exact time, the previous index is returned. If the time is
397 |             before or after the first and last message times, the first or last
398 |             index and message are returned
399 | 
400 |         """
401 |         # Gives the index to insert into to retain a sorted queue. The topic queues
402 |         # should always be sorted due to time passing.
403 | 
404 |         # ind = bisect.bisect(queue, self.Message(stamp=t, message=''))
405 |         # Can't use bisect here in python3 unless the correct operators
406 |         # are defined for sorting, so do it manually
407 |         try:
408 |             ind = next(i for i, msg in enumerate(queue) if t < msg.stamp)
409 |         except StopIteration:
410 |             ind = len(queue)
411 | 
412 |         # first or last indices
413 |         if ind == len(queue):
414 |             return (ind - 1, queue[-1])
415 |         elif ind == 0:
416 |             return (0, queue[0])
417 | 
418 |         # non-end indices
419 |         cur = queue[ind]
420 |         if cur.stamp == t:
421 |             return (ind, cur)
422 |         else:
423 |             return (ind - 1, queue[ind - 1])
424 | 
425 |     def get_entry(self, t, topic):
426 |         """Get a message in the queues for a specific topic
427 |         :param ``rospy.Time`` t: time of the message to retrieve
428 |         :param str topic: the topic to be accessed
429 |         :return: message corresponding to time t and topic. If there is no
430 |             corresponding entry at exactly the given time, the latest entry
431 |             before the given time is returned. If the topic does not exist, or
432 |             there is no entry, None.
433 | 
434 |         """
435 |         with self._topic_lock:
436 |             entry = None
437 |             if topic in self._topics:
438 |                 _, entry = self._entry_at(t, self._topics[topic].queue)
439 | 
440 |             return entry
441 | 
442 |     def get_entry_before(self, t):
443 |         """
444 |         Get the latest message before the given time on any of the topics
445 |         :param t: time, ``rospy.Time``
446 |         :return: tuple of (topic, entry) corresponding to time t, ``(str, msg)``
447 |         """
448 |         with self._topic_lock:
449 |             entry_topic, entry = None, None
450 |             for topic in self._topics:
451 |                 _, topic_entry = self._entry_at(t - rospy.Duration(0,1), self._topics[topic].queue)
452 |                 if topic_entry and (not entry or topic_entry.stamp > entry.stamp):
453 |                     entry_topic, entry = topic, topic_entry
454 | 
455 |             return entry_topic, entry
456 | 
457 |     def get_entry_after(self, t):
458 |         """
459 |         Get the earliest message on any topic after the given time
460 |         :param t: time, ''rospy.Time''
461 |         :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
462 |         """
463 |         with self._topic_lock:
464 |             entry_topic, entry = None, None
465 |             for topic in self._topics:
466 |                 ind, _ = self._entry_at(t, self._topics[topic].queue)
467 |                 # ind is the index of the entry at (if it exists) or before time
468 |                 # t - we want the one after this. Make sure that the given index
469 |                 # isn't out of bounds
470 |                 ind = ind + 1 if ind + 1 < len(self._topics[topic].queue) else ind
471 |                 topic_entry = self._topics[topic].queue[ind]
472 |                 if topic_entry and (not entry or topic_entry.stamp < entry.stamp):
473 |                     entry_topic, entry = topic, topic_entry
474 | 
475 |             return entry_topic, entry
476 | 
477 |     def get_next_message_time(self):
478 |         """
479 |         :return: time of the message after the current playhead position,''rospy.Time''
480 |         """
481 |         if self._timeline_frame.playhead is None:
482 |             return None
483 | 
484 |         _, entry = self.get_entry_after(self._timeline_frame.playhead)
485 |         if entry is None:
486 |             return self._timeline_frame._end_stamp
487 | 
488 |         return entry.stamp
489 | 
490 |     def get_previous_message_time(self):
491 |         """
492 |         :return: time of the message before the current playhead position,''rospy.Time''
493 |         """
494 |         if self._timeline_frame.playhead is None:
495 |             return None
496 | 
497 |         _, entry = self.get_entry_before(self._timeline_frame.playhead)
498 |         if entry is None:
499 |             return self._timeline_frame._start_stamp
500 | 
501 |         return entry.stamp
502 | 
503 |     def resume(self):
504 |         if (self._player):
505 |             self._player.resume()
506 | 
507 |     ### Copy messages to...
508 | 
509 |     def start_background_task(self, background_task):
510 |         """
511 |         Verify that a background task is not currently running before starting a new one
512 |         :param background_task: name of the background task, ''str''
513 |         """
514 |         if self.background_task is not None:
515 |             QMessageBox(QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_()
516 |             return False
517 | 
518 |         self.background_task = background_task
519 |         self.background_task_cancel = False
520 |         return True
521 | 
522 |     def stop_background_task(self):
523 |         self.background_task = None
524 | 
525 |     def copy_region_to_bag(self, filename):
526 |         if len(self._bags) > 0:
527 |             self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1])
528 | 
529 |     def _export_region(self, path, topics, start_stamp, end_stamp):
530 |         """
531 |         Starts a thread to save the current selection to a new bag file
532 |         :param path: filesystem path to write to, ''str''
533 |         :param topics: topics to write to the file, ''list(str)''
534 |         :param start_stamp: start of area to save, ''rospy.Time''
535 |         :param end_stamp: end of area to save, ''rospy.Time''
536 |         """
537 |         if not self.start_background_task('Copying messages to "%s"' % path):
538 |             return
539 |         # TODO implement a status bar area with information on the current save status
540 |         bag_entries = list(self.get_entries_with_bags(topics, start_stamp, end_stamp))
541 | 
542 |         if self.background_task_cancel:
543 |             return
544 | 
545 |         # Get the total number of messages to copy
546 |         total_messages = len(bag_entries)
547 | 
548 |         # If no messages, prompt the user and return
549 |         if total_messages == 0:
550 |             QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_()
551 |             self.stop_background_task()
552 |             return
553 | 
554 |         # Open the path for writing
555 |         try:
556 |             export_bag = rosbag.Bag(path, 'w')
557 |         except Exception:
558 |             QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_()
559 |             self.stop_background_task()
560 |             return
561 | 
562 |         # Run copying in a background thread
563 |         self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries))
564 |         self._export_thread.start()
565 | 
566 |     def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries):
567 |         """
568 |         Threaded function that saves the current selection to a new bag file
569 |         :param export_bag: bagfile to write to, ''rosbag.bag''
570 |         :param topics: topics to write to the file, ''list(str)''
571 |         :param start_stamp: start of area to save, ''rospy.Time''
572 |         :param end_stamp: end of area to save, ''rospy.Time''
573 |         """
574 |         total_messages = len(bag_entries)
575 |         update_step = max(1, total_messages / 100)
576 |         message_num = 1
577 |         progress = 0
578 |         # Write out the messages
579 |         for bag, entry in bag_entries:
580 |             if self.background_task_cancel:
581 |                 break
582 |             try:
583 |                 topic, msg, t = self.read_message(bag, entry.position)
584 |                 export_bag.write(topic, msg, t)
585 |             except Exception as ex:
586 |                 qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex)))
587 |                 export_bag.close()
588 |                 self.stop_background_task()
589 |                 return
590 | 
591 |             if message_num % update_step == 0 or message_num == total_messages:
592 |                 new_progress = int(100.0 * (float(message_num) / total_messages))
593 |                 if new_progress != progress:
594 |                     progress = new_progress
595 |                     if not self.background_task_cancel:
596 |                         self.background_progress = progress
597 |                         self.status_bar_changed_signal.emit()
598 | 
599 |             message_num += 1
600 | 
601 |         # Close the bag
602 |         try:
603 |             self.background_progress = 0
604 |             self.status_bar_changed_signal.emit()
605 |             export_bag.close()
606 |         except Exception as ex:
607 |             QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
608 |         self.stop_background_task()
609 | 
610 |     def read_message(self, topic, position):
611 |         with self._topic_lock:
612 |             return self.get_entry(position, topic).message
613 | 
614 |     def on_mouse_down(self, event):
615 |         """
616 |         When the user clicks down in the timeline.
617 |         """
618 |         if event.buttons() == Qt.LeftButton:
619 |             self._timeline_frame.on_left_down(event)
620 |         elif event.buttons() == Qt.MidButton:
621 |             self._timeline_frame.on_middle_down(event)
622 |         elif event.buttons() == Qt.RightButton:
623 |             topic = self._timeline_frame.map_y_to_topic(event.y())
624 |             TimelinePopupMenu(self, event, topic)
625 | 
626 |     def on_mouse_up(self, event):
627 |         self._timeline_frame.on_mouse_up(event)
628 | 
629 |     def on_mouse_move(self, event):
630 |         self._timeline_frame.on_mouse_move(event)
631 | 
632 |     def on_mousewheel(self, event):
633 |         self._timeline_frame.on_mousewheel(event)
634 | 
635 |     # Zooming
636 | 
637 |     def zoom_in(self):
638 |         self._timeline_frame.zoom_in()
639 | 
640 |     def zoom_out(self):
641 |         self._timeline_frame.zoom_out()
642 | 
643 |     def reset_zoom(self):
644 |         self._timeline_frame.reset_zoom()
645 | 
646 |     def translate_timeline_left(self):
647 |         self._timeline_frame.translate_timeline_left()
648 | 
649 |     def translate_timeline_right(self):
650 |         self._timeline_frame.translate_timeline_right()
651 | 
652 |     ### Publishing
653 |     def is_publishing(self, topic):
654 |         return self._player and self._player.is_publishing(topic)
655 | 
656 |     def start_publishing(self, topic):
657 |         if not self._player and not self._create_player():
658 |             return False
659 | 
660 |         self._player.start_publishing(topic)
661 |         return True
662 | 
663 |     def stop_publishing(self, topic):
664 |         if not self._player:
665 |             return False
666 | 
667 |         self._player.stop_publishing(topic)
668 |         return True
669 | 
670 |     def _create_player(self):
671 |         if not self._player:
672 |             try:
673 |                 self._player = Player(self)
674 |                 if self._publish_clock:
675 |                     self._player.start_clock_publishing()
676 |             except Exception as ex:
677 |                 qWarning('Error starting player; aborting publish: %s' % str(ex))
678 |                 return False
679 | 
680 |         return True
681 | 
682 |     def set_publishing_state(self, start_publishing):
683 |         if start_publishing:
684 |             for topic in self._timeline_frame.topics:
685 |                 if not self.start_publishing(topic):
686 |                     break
687 |         else:
688 |             for topic in self._timeline_frame.topics:
689 |                 self.stop_publishing(topic)
690 | 
691 |     # property: play_all
692 |     def _get_play_all(self):
693 |         return self._play_all
694 | 
695 |     def _set_play_all(self, play_all):
696 |         if play_all == self._play_all:
697 |             return
698 | 
699 |         self._play_all = not self._play_all
700 | 
701 |         self.last_frame = None
702 |         self.last_playhead = None
703 |         self.desired_playhead = None
704 | 
705 |     play_all = property(_get_play_all, _set_play_all)
706 | 
707 |     def toggle_play_all(self):
708 |         self.play_all = not self.play_all
709 | 
710 |     ### Playing
711 |     def on_idle(self):
712 |         self._step_playhead()
713 | 
714 |     def _step_playhead(self):
715 |         """
716 |         moves the playhead to the next position based on the desired position
717 |         """
718 |         # Reset when the playing mode switchs
719 |         if self._timeline_frame.playhead != self.last_playhead:
720 |             self.last_frame = None
721 |             self.last_playhead = None
722 |             self.desired_playhead = None
723 | 
724 |         if self._play_all:
725 |             self.step_next_message()
726 |         else:
727 |             self.step_fixed()
728 | 
729 |     def step_fixed(self):
730 |         """
731 |         Moves the playhead a fixed distance into the future based on the current play speed
732 |         """
733 |         if self.play_speed == 0.0 or not self._timeline_frame.playhead:
734 |             self.last_frame = None
735 |             self.last_playhead = None
736 |             return
737 | 
738 |         now = rospy.Time.from_sec(time.time())
739 |         if self.last_frame:
740 |             # Get new playhead
741 |             if self.stick_to_end:
742 |                 new_playhead = self.end_stamp
743 |             else:
744 |                 new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed)
745 | 
746 |                 start_stamp, end_stamp = self._timeline_frame.play_region
747 | 
748 |                 if new_playhead > end_stamp:
749 |                     if self.wrap:
750 |                         if self.play_speed > 0.0:
751 |                             new_playhead = start_stamp
752 |                         else:
753 |                             new_playhead = end_stamp
754 |                     else:
755 |                         new_playhead = end_stamp
756 | 
757 |                         if self.play_speed > 0.0:
758 |                             self.stick_to_end = True
759 | 
760 |                 elif new_playhead < start_stamp:
761 |                     if self.wrap:
762 |                         if self.play_speed < 0.0:
763 |                             new_playhead = end_stamp
764 |                         else:
765 |                             new_playhead = start_stamp
766 |                     else:
767 |                         new_playhead = start_stamp
768 | 
769 |             # Update the playhead
770 |             self._timeline_frame.playhead = new_playhead
771 | 
772 |         self.last_frame = now
773 |         self.last_playhead = self._timeline_frame.playhead
774 | 
775 |     def step_next_message(self):
776 |         """
777 |         Move the playhead to the next message
778 |         """
779 |         if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
780 |             self.last_frame = None
781 |             self.last_playhead = None
782 |             return
783 | 
784 |         if self.last_frame:
785 |             if not self.desired_playhead:
786 |                 self.desired_playhead = self._timeline_frame.playhead
787 |             else:
788 |                 delta = rospy.Time.from_sec(time.time()) - self.last_frame
789 |                 if delta > rospy.Duration.from_sec(0.1):
790 |                     delta = rospy.Duration.from_sec(0.1)
791 |                 self.desired_playhead += delta
792 | 
793 |             # Get the occurrence of the next message
794 |             next_message_time = self.get_next_message_time()
795 | 
796 |             if next_message_time < self.desired_playhead:
797 |                 self._timeline_frame.playhead = next_message_time
798 |             else:
799 |                 self._timeline_frame.playhead = self.desired_playhead
800 | 
801 |         self.last_frame = rospy.Time.from_sec(time.time())
802 |         self.last_playhead = self._timeline_frame.playhead
803 | 
804 |     # Recording
805 |     def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
806 |         try:
807 |             self._recorder = Recorder(filename, bag_lock=self._bag_lock, all=all, topics=topics, regex=regex, limit=limit)
808 |         except Exception as ex:
809 |             qWarning('Error opening bag for recording [%s]: %s' % (filename, str(ex)))
810 |             return
811 | 
812 |         self._recorder.add_listener(self._message_recorded)
813 | 
814 |         self.add_bag(self._recorder.bag)
815 | 
816 |         self._recorder.start()
817 | 
818 |         self.wrap = False
819 |         self._timeline_frame._index_cache_thread.period = 0.1
820 | 
821 |         self.update()
822 | 
823 |     def toggle_recording(self):
824 |         if self._recorder:
825 |             self._recorder.toggle_paused()
826 |             self.update()
827 | 
828 |     def _message_recorded(self, topic, msg, t):
829 |         if self._timeline_frame._start_stamp is None:
830 |             self._timeline_frame._start_stamp = t
831 |             self._timeline_frame._end_stamp = t
832 |             self._timeline_frame._playhead = t
833 |         elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
834 |             self._timeline_frame._end_stamp = t
835 | 
836 |         if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
837 |             self._timeline_frame.topics = self._get_topics()
838 |             self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype()
839 | 
840 |             self._playhead_positions_cvs[topic] = threading.Condition()
841 |             self._messages_cvs[topic] = threading.Condition()
842 |             self._message_loaders[topic] = MessageLoaderThread(self, topic)
843 | 
844 |         if self._timeline_frame._stamp_left is None:
845 |             self.reset_zoom()
846 | 
847 |         # Notify the index caching thread that it has work to do
848 |         with self._timeline_frame.index_cache_cv:
849 |             self._timeline_frame.invalidated_caches.add(topic)
850 |             self._timeline_frame.index_cache_cv.notify()
851 | 
852 |         if topic in self._listeners:
853 |             for listener in self._listeners[topic]:
854 |                 try:
855 |                     listener.timeline_changed()
856 |                 except Exception as ex:
857 |                     qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex)))
858 | 
859 |     # Views / listeners
860 |     def add_view(self, topic, frame):
861 |         self._views.append(frame)
862 | 
863 |     def has_listeners(self, topic):
864 |         return topic in self._listeners
865 | 
866 |     def add_listener(self, topic, listener):
867 |         self._listeners.setdefault(topic, []).append(listener)
868 | 
869 |         self._message_listener_threads[(topic, listener)] = MessageListenerThread(self, topic, listener)
870 |         # Notify the message listeners
871 |         self._message_loaders[topic].reset()
872 |         with self._playhead_positions_cvs[topic]:
873 |             self._playhead_positions_cvs[topic].notify_all()
874 | 
875 |         self.update()
876 | 
877 |     def remove_listener(self, topic, listener):
878 |         topic_listeners = self._listeners.get(topic)
879 |         if topic_listeners is not None and listener in topic_listeners:
880 |             topic_listeners.remove(listener)
881 | 
882 |             if len(topic_listeners) == 0:
883 |                 del self._listeners[topic]
884 | 
885 |             # Stop the message listener thread
886 |             if (topic, listener) in self._message_listener_threads:
887 |                 self._message_listener_threads[(topic, listener)].stop()
888 |                 del self._message_listener_threads[(topic, listener)]
889 |             self.update()
890 | 
891 |     ### Playhead
892 | 
893 |     # property: play_speed
894 |     def _get_play_speed(self):
895 |         if self._timeline_frame._paused:
896 |             return 0.0
897 |         return self._play_speed
898 | 
899 |     def _set_play_speed(self, play_speed):
900 |         if play_speed == self._play_speed:
901 |             return
902 | 
903 |         if play_speed > 0.0:
904 |             self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed))
905 |         elif play_speed < 0.0:
906 |             self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed))
907 |         else:
908 |             self._play_speed = play_speed
909 | 
910 |         if self._play_speed < 1.0:
911 |             self.stick_to_end = False
912 | 
913 |         self.update()
914 |     play_speed = property(_get_play_speed, _set_play_speed)
915 | 
916 |     def toggle_play(self):
917 |         if self._play_speed != 0.0:
918 |             self.play_speed = 0.0
919 |         else:
920 |             self.play_speed = 1.0
921 | 
922 |     def navigate_play(self):
923 |         self.play_speed = 1.0
924 |         self.last_frame = rospy.Time.from_sec(time.time())
925 |         self.last_playhead = self._timeline_frame.playhead
926 |         self._play_timer.start()
927 | 
928 |     def navigate_stop(self):
929 |         self.play_speed = 0.0
930 |         self._play_timer.stop()
931 | 
932 |     def navigate_previous(self):
933 |         self.navigate_stop()
934 |         self._timeline_frame.playhead = self.get_previous_message_time()
935 |         self.last_playhead = self._timeline_frame.playhead
936 | 
937 |     def navigate_next(self):
938 |         self.navigate_stop()
939 |         self._timeline_frame.playhead = self.get_next_message_time()
940 |         self.last_playhead = self._timeline_frame.playhead
941 | 
942 |     def navigate_rewind(self):
943 |         if self._play_speed < 0.0:
944 |             new_play_speed = self._play_speed * 2.0
945 |         elif self._play_speed == 0.0:
946 |             new_play_speed = -1.0
947 |         else:
948 |             new_play_speed = self._play_speed * 0.5
949 | 
950 |         self.play_speed = new_play_speed
951 | 
952 |     def navigate_fastforward(self):
953 |         if self._play_speed > 0.0:
954 |             new_play_speed = self._play_speed * 2.0
955 |         elif self._play_speed == 0.0:
956 |             new_play_speed = 2.0
957 |         else:
958 |             new_play_speed = self._play_speed * 0.5
959 | 
960 |         self.play_speed = new_play_speed
961 | 
962 |     def navigate_start(self):
963 |         self._timeline_frame.playhead = self._timeline_frame.play_region[0]
964 | 
965 |     def navigate_end(self):
966 |         self._timeline_frame.playhead = self._timeline_frame.play_region[1]
967 | 


--------------------------------------------------------------------------------
/src/rqt_py_trees/behaviour_tree.py:
--------------------------------------------------------------------------------
  1 | # Software License Agreement (BSD License)
  2 | #
  3 | # Copyright (c) 2011, Willow Garage, Inc.
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions
  8 | # are met:
  9 | #
 10 | #  * Redistributions of source code must retain the above copyright
 11 | #    notice, this list of conditions and the following disclaimer.
 12 | #  * Redistributions in binary form must reproduce the above
 13 | #    copyright notice, this list of conditions and the following
 14 | #    disclaimer in the documentation and/or other materials provided
 15 | #    with the distribution.
 16 | #  * Neither the name of Willow Garage, Inc. nor the names of its
 17 | #    contributors may be used to endorse or promote products derived
 18 | #    from this software without specific prior written permission.
 19 | #
 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 31 | # POSSIBILITY OF SUCH DAMAGE.
 32 | 
 33 | from __future__ import division
 34 | 
 35 | import argparse
 36 | import fnmatch
 37 | import functools
 38 | import py_trees
 39 | import py_trees_msgs.msg as py_trees_msgs
 40 | import os
 41 | import re
 42 | import rosbag
 43 | import rospkg
 44 | import rospy
 45 | import sys
 46 | import termcolor
 47 | import uuid_msgs.msg as uuid_msgs
 48 | 
 49 | from . import visibility
 50 | 
 51 | from .dotcode_behaviour import RosBehaviourTreeDotcodeGenerator
 52 | from .dynamic_timeline import DynamicTimeline
 53 | from .dynamic_timeline_listener import DynamicTimelineListener
 54 | from .timeline_listener import TimelineListener
 55 | from .qt_dotgraph.dot_to_qt import DotToQtGenerator
 56 | from .qt_dotgraph.pydotfactory import PydotFactory
 57 | from .qt_dotgraph.pygraphvizfactory import PygraphvizFactory
 58 | from rqt_bag.bag_timeline import BagTimeline
 59 | # from rqt_bag.bag_widget import BagGraphicsView
 60 | from rqt_graph.interactive_graphics_view import InteractiveGraphicsView
 61 | 
 62 | from python_qt_binding import loadUi
 63 | from python_qt_binding.QtCore import QFile, QIODevice, QObject, Qt, Signal, QEvent, Slot
 64 | from python_qt_binding.QtGui import QIcon, QImage, QPainter, QKeySequence
 65 | from python_qt_binding.QtSvg import QSvgGenerator
 66 | try:  # indigo
 67 |     from python_qt_binding.QtGui import QFileDialog, QGraphicsView, QGraphicsScene, QWidget, QShortcut
 68 | except ImportError:  # kinetic+ (pyqt5)
 69 |     from python_qt_binding.QtWidgets import QFileDialog, QGraphicsView, QGraphicsScene, QWidget, QShortcut
 70 | 
 71 | 
 72 | class RosBehaviourTree(QObject):
 73 | 
 74 |     _deferred_fit_in_view = Signal()
 75 |     _refresh_view = Signal()
 76 |     _refresh_combo = Signal()
 77 |     _message_changed = Signal()
 78 |     _message_cleared = Signal()
 79 |     _expected_type = py_trees_msgs.BehaviourTree()._type
 80 |     _empty_topic = "No valid topics available"
 81 |     _unselected_topic = "Not subscribing"
 82 |     no_roscore_switch = "--no-roscore"
 83 | 
 84 |     class ComboBoxEventFilter(QObject):
 85 |         """Event filter for the combo box. Will filter left mouse button presses,
 86 |         calling a signal when they happen
 87 | 
 88 |         """
 89 |         def __init__(self, signal):
 90 |             """
 91 | 
 92 |             :param Signal signal: signal that is emitted when a left mouse button press happens
 93 |             """
 94 |             super(RosBehaviourTree.ComboBoxEventFilter, self).__init__()
 95 |             self.signal = signal
 96 | 
 97 |         def eventFilter(self, obj, event):
 98 |             if event.type() == QEvent.MouseButtonPress and event.button() == Qt.LeftButton:
 99 |                 self.signal.emit()
100 |             return False
101 | 
102 |     def __init__(self, context):
103 |         super(RosBehaviourTree, self).__init__(context)
104 |         self.setObjectName('RosBehaviourTree')
105 | 
106 |         parser = argparse.ArgumentParser()
107 |         RosBehaviourTree.add_arguments(parser, False)
108 |         # if the context doesn't have an argv attribute then assume we're running with --no-roscore
109 |         if not hasattr(context, 'argv'):
110 |             args = sys.argv[1:]
111 |             # Can run the viewer with or without live updating. Running without is
112 |             # intended for viewing of bags only
113 |             self.live_update = False
114 |         else:
115 |             args = context.argv()
116 |             self.live_update = True
117 | 
118 |         parsed_args = parser.parse_args(args)
119 | 
120 |         self.context = context
121 |         self.initialized = False
122 |         self._current_dotcode = None  # dotcode for the tree that is currently displayed
123 |         self._viewing_bag = False  # true if a bag file is loaded
124 |         # True if next or previous buttons are pressed. Reset if the tree being
125 |         # viewed is the last one in the list.
126 |         self._browsing_timeline = False
127 | 
128 |         self._widget = QWidget()
129 | 
130 |         # factory builds generic dotcode items
131 |         self.dotcode_factory = PygraphvizFactory()  # PydotFactory()
132 |         # self.dotcode_factory = PygraphvizFactory()
133 |         # generator builds rosgraph
134 |         self.dotcode_generator = RosBehaviourTreeDotcodeGenerator()
135 |         self.current_topic = None
136 |         self.behaviour_sub = None
137 |         self._tip_message = None  # message of the tip of the tree
138 |         self._default_topic = parsed_args.topic  # topic parsed on the command line
139 |         self._saved_settings_topic = None  # topic subscribed to by previous instance
140 |         self.visibility_level = py_trees.common.VisibilityLevel.DETAIL
141 | 
142 |         # dot_to_qt transforms into Qt elements using dot layout
143 |         self.dot_to_qt = DotToQtGenerator()
144 | 
145 |         rp = rospkg.RosPack()
146 |         ui_file = os.path.join(rp.get_path('rqt_py_trees'), 'resource', 'RosBehaviourTree.ui')
147 |         loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
148 |         self._widget.setObjectName('RosBehaviourTreeUi')
149 |         if hasattr(context, 'serial_number') and context.serial_number() > 1:
150 |             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
151 | 
152 |         self._scene = QGraphicsScene()
153 |         self._scene.setBackgroundBrush(Qt.white)
154 |         self._widget.graphics_view.setScene(self._scene)
155 | 
156 |         self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
157 |         self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
158 |         self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
159 |         self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
160 | 
161 |         self._widget.load_bag_push_button.setIcon(QIcon.fromTheme('document-open'))
162 |         self._widget.load_bag_push_button.pressed.connect(self._load_bag)
163 |         self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
164 |         self._widget.load_dot_push_button.pressed.connect(self._load_dot)
165 |         self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
166 |         self._widget.save_dot_push_button.pressed.connect(self._save_dot)
167 |         self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
168 |         self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
169 |         self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
170 |         self._widget.save_as_image_push_button.pressed.connect(self._save_image)
171 | 
172 |         for text in visibility.combo_to_py_trees:
173 |             self._widget.visibility_level_combo_box.addItem(text)
174 |         self._widget.visibility_level_combo_box.setCurrentIndex(self.visibility_level)
175 |         self._widget.visibility_level_combo_box.currentIndexChanged['QString'].connect(self._update_visibility_level)
176 | 
177 |         # set up the function that is called whenever the box is resized -
178 |         # ensures that the timeline is correctly drawn.
179 |         self._widget.resizeEvent = self._resize_event
180 | 
181 |         self._timeline = None
182 |         self._timeline_listener = None
183 | 
184 |         # Connect the message changed function of this object to a corresponding
185 |         # signal. This signal will be activated whenever the message being
186 |         # viewed changes.
187 |         self._message_changed.connect(self.message_changed)
188 |         self._message_cleared.connect(self.message_cleared)
189 | 
190 |         # Set up combo box for topic selection
191 |         # when the refresh_combo signal happens, update the combo topics available
192 |         self._refresh_combo.connect(self._update_combo_topics)
193 |         # filter events to catch the event which opens the combo box
194 |         self._combo_event_filter = RosBehaviourTree.ComboBoxEventFilter(self._refresh_combo)
195 |         self._widget.topic_combo_box.installEventFilter(self._combo_event_filter)
196 |         self._widget.topic_combo_box.activated.connect(self._choose_topic)
197 |         # Delay populating the combobox, let restore_settings() handle it
198 |         # self._update_combo_topics()
199 | 
200 |         # Set up navigation buttons
201 |         self._widget.previous_tool_button.pressed.connect(self._previous)
202 |         self._widget.previous_tool_button.setIcon(QIcon.fromTheme('go-previous'))
203 |         self._widget.next_tool_button.pressed.connect(self._next)
204 |         self._widget.next_tool_button.setIcon(QIcon.fromTheme('go-next'))
205 |         self._widget.first_tool_button.pressed.connect(self._first)
206 |         self._widget.first_tool_button.setIcon(QIcon.fromTheme('go-first'))
207 |         self._widget.last_tool_button.pressed.connect(self._last)
208 |         self._widget.last_tool_button.setIcon(QIcon.fromTheme('go-last'))
209 | 
210 |         # play, pause and stop buttons
211 |         self._widget.play_tool_button.pressed.connect(self._play)
212 |         self._widget.play_tool_button.setIcon(QIcon.fromTheme('media-playback-start'))
213 |         self._widget.stop_tool_button.pressed.connect(self._stop)
214 |         self._widget.stop_tool_button.setIcon(QIcon.fromTheme('media-playback-stop'))
215 |         # also connect the navigation buttons so that they stop the timer when
216 |         # pressed while the tree is playing.
217 |         self._widget.first_tool_button.pressed.connect(self._stop)
218 |         self._widget.previous_tool_button.pressed.connect(self._stop)
219 |         self._widget.last_tool_button.pressed.connect(self._stop)
220 |         self._widget.next_tool_button.pressed.connect(self._stop)
221 | 
222 |         # set up shortcuts for navigation (vim)
223 |         next_shortcut_vi = QShortcut(QKeySequence("l"), self._widget)
224 |         next_shortcut_vi.activated.connect(self._widget.next_tool_button.pressed)
225 |         previous_shortcut_vi = QShortcut(QKeySequence("h"), self._widget)
226 |         previous_shortcut_vi.activated.connect(self._widget.previous_tool_button.pressed)
227 |         first_shortcut_vi = QShortcut(QKeySequence("^"), self._widget)
228 |         first_shortcut_vi.activated.connect(self._widget.first_tool_button.pressed)
229 |         last_shortcut_vi = QShortcut(QKeySequence("$"), self._widget)
230 |         last_shortcut_vi.activated.connect(self._widget.last_tool_button.pressed)
231 | 
232 |         # shortcuts for emacs
233 |         next_shortcut_emacs = QShortcut(QKeySequence("Ctrl+f"), self._widget)
234 |         next_shortcut_emacs.activated.connect(self._widget.next_tool_button.pressed)
235 |         previous_shortcut_emacs = QShortcut(QKeySequence("Ctrl+b"), self._widget)
236 |         previous_shortcut_emacs.activated.connect(self._widget.previous_tool_button.pressed)
237 |         first_shortcut_emacs = QShortcut(QKeySequence("Ctrl+a"), self._widget)
238 |         first_shortcut_emacs.activated.connect(self._widget.first_tool_button.pressed)
239 |         last_shortcut_emacs = QShortcut(QKeySequence("Ctrl+e"), self._widget)
240 |         last_shortcut_emacs.activated.connect(self._widget.last_tool_button.pressed)
241 | 
242 |         # set up stuff for dotcode cache
243 |         self._dotcode_cache_capacity = 50
244 |         self._dotcode_cache = {}
245 |         # cache is ordered on timestamps from messages, but earliest timestamp
246 |         # isn't necessarily the message that was viewed the longest time ago, so
247 |         # need to store keys
248 |         self._dotcode_cache_keys = []
249 | 
250 |         # set up stuff for scene cache (dotcode cache doesn't seem to make much difference)
251 |         self._scene_cache_capacity = 50
252 |         self._scene_cache = {}
253 |         self._scene_cache_keys = []
254 | 
255 |         # Update the timeline buttons to correspond with a completely
256 |         # uninitialised state.
257 |         self._set_timeline_buttons(first_snapshot=False, previous_snapshot=False, next_snapshot=False, last_snapshot=False)
258 | 
259 |         self._deferred_fit_in_view.connect(self._fit_in_view,
260 |                                            Qt.QueuedConnection)
261 |         self._deferred_fit_in_view.emit()
262 | 
263 |         # This is used to store a timer which controls how fast updates happen when the play button is pressed.
264 |         self._play_timer = None
265 | 
266 |         # updates the view
267 |         self._refresh_view.connect(self._refresh_tree_graph)
268 | 
269 |         self._force_refresh = False
270 | 
271 |         if self.live_update:
272 |             context.add_widget(self._widget)
273 |         else:
274 |             self.initialized = True  # this needs to be set for trees to be displayed
275 |             context.setCentralWidget(self._widget)
276 | 
277 |         if parsed_args.bag:
278 |             self._load_bag(parsed_args.bag)
279 |         elif parsed_args.latest_bag:
280 |             # if the latest bag is requested, load it from the default directory, or
281 |             # the one specified in the args
282 |             bag_dir = parsed_args.bag_dir or os.getenv('ROS_HOME', os.path.expanduser('~/.ros')) + '/behaviour_trees'
283 |             self.open_latest_bag(bag_dir, parsed_args.by_time)
284 | 
285 |     @Slot(str)
286 |     def _update_visibility_level(self, visibility_level):
287 |         """
288 |         We match the combobox index to the visibility levels defined in py_trees.common.VisibilityLevel.
289 |         """
290 |         self.visibility_level = visibility.combo_to_py_trees[visibility_level]
291 |         self._refresh_tree_graph()
292 | 
293 |     @staticmethod
294 |     def add_arguments(parser, group=True):
295 |         """Allows for the addition of arguments to the rqt_gui loading method
296 | 
297 |         :param bool group: If set to false, this indicates that the function is
298 |             being called from the rqt_py_trees script as opposed to the inside
299 |             of rqt_gui.main. We use this to ensure that the same arguments can
300 |             be passed with and without the --no-roscore argument set. If it is
301 |             set, the rqt_gui code is bypassed. We need to make sure that all the
302 |             arguments are displayed with -h.
303 | 
304 |         """
305 |         operate_object = parser
306 |         if group:
307 |             operate_object = parser.add_argument_group('Options for the rqt_py_trees viewer')
308 | 
309 |         operate_object.add_argument('bag', action='store', nargs='?', help='Load this bag when the viewer starts')
310 |         operate_object.add_argument('-l', '--latest-bag', action='store_true', help='Load the latest bag available in the bag directory. Bag files are expected to be under the bag directory in the following structure: year-month-day/behaviour_tree_hour-minute-second.bag. If this structure is not followed, the bag file which was most recently modified is used.')
311 |         operate_object.add_argument('--bag-dir', action='store', help='Specify the directory in which to look for bag files. The default is $ROS_HOME/behaviour_trees, if $ROS_HOME is set, or ~/.ros/behaviour_trees otherwise.')
312 |         operate_object.add_argument('-m', '--by-time', action='store_true', help='The latest bag is defined by the time at which the file was last modified, rather than the date and time specified in the filename.')
313 |         operate_object.add_argument(RosBehaviourTree.no_roscore_switch, action='store_true', help='Run the viewer without roscore. It is only possible to view bag files if this is set.')
314 |         operate_object.add_argument('--topic', action='store', type=str, default=None, help='Default topic to defer to [default:None]')
315 | 
316 |     def open_latest_bag(self, bag_dir, by_time=False):
317 |         """Open the latest bag in the given directory
318 | 
319 |         :param str bag_dir: the directory in which to look for bags
320 |         :param bool by_time: if true, the latest bag is the one with the latest
321 |             modification time, not the latest date-time specified by its filename
322 | 
323 |         """
324 |         if not os.path.isdir(bag_dir):
325 |             rospy.logwarn("Requested bag directory {0} is invalid. Latest bag will not be loaded.".format(bag_dir))
326 |             return
327 | 
328 |         files = []
329 |         for root, unused_dirnames, filenames in os.walk(bag_dir, topdown=True):
330 |             files.extend(fnmatch.filter(map(lambda p: os.path.join(root, p), filenames), '*.bag'))
331 | 
332 |         if not files:
333 |             rospy.logwarn("No files with extension .bag found in directory {0}".format(bag_dir))
334 |             return
335 | 
336 |         if not by_time:
337 |             # parse the file list with a regex to get only those which have the
338 |             # format year-month-day/behaviour_tree_hour-minute-second.bag
339 |             re_str = '.*\/\d{4}-\d{2}-\d{2}\/behaviour_tree_\d{2}-\d{2}-\d{2}.bag'
340 |             expr = re.compile(re_str)
341 |             valid = filter(lambda f: expr.match(f), files)
342 | 
343 |             # if no files match the regex, use modification time instead
344 |             if not valid:
345 |                 by_time = True
346 |             else:
347 |                 # dates are monotonically increasing, so the last one is the latest
348 |                 latest_bag = sorted(valid)[-1]
349 | 
350 |         if by_time:
351 |             latest_bag = sorted(files, cmp=lambda x, y: cmp(os.path.getctime(x), os.path.getctime(y)))[-1]
352 | 
353 |         self._load_bag(latest_bag)
354 | 
355 |     def get_current_message(self):
356 |         """
357 |         Get the message in the list or bag that is being viewed that should be
358 |         displayed.
359 |         """
360 |         msg = None
361 |         if self._timeline_listener:
362 |             try:
363 |                 msg = self._timeline_listener.msg
364 |             except KeyError:
365 |                 pass
366 | 
367 |         return py_trees_msgs.BehaviourTree() if msg is None else msg
368 | 
369 |     def _choose_topic(self, index):
370 |         """Updates the topic that is subscribed to based on changes to the combo box
371 |         text. If the topic is unchanged, nothing will happnen. Otherwise, the
372 |         old subscriber will be unregistered, and a new one initialised for the
373 |         updated topic. If the selected topic corresponds to the unselected
374 |         topic, the subscriber will be unregistered and a new one will not be
375 |         created.
376 | 
377 |         """
378 |         selected_topic = self._widget.topic_combo_box.currentText()
379 |         if selected_topic != self._empty_topic and self.current_topic != selected_topic:
380 |             self.current_topic = selected_topic
381 |             # destroy the old timeline and clear the scene
382 |             if self._timeline:
383 |                 self._timeline.handle_close()
384 |                 self._widget.timeline_graphics_view.setScene(None)
385 | 
386 |             if selected_topic != self._unselected_topic:
387 |                 # set up a timeline to track the messages coming from the subscriber
388 |                 self._set_dynamic_timeline()
389 | 
390 |     def _update_combo_topics(self):
391 |         """
392 |         Update the topics displayed in the combo box that the user can use to select
393 |         which topic they want to listen on for trees, filtered so that only
394 |         topics with the correct message type are shown.
395 | 
396 |         This method is triggered on startup and on user combobox interactions
397 |         """
398 |         # Only update topics if we're running with live update (i.e. with roscore)
399 |         if not self.live_update:
400 |             self._widget.topic_combo_box.setEnabled(False)
401 |             return
402 | 
403 |         self._widget.topic_combo_box.clear()
404 |         topic_list = rospy.get_published_topics()
405 | 
406 |         valid_topics = []
407 |         for topic_path, topic_type in topic_list:
408 |             if topic_type == RosBehaviourTree._expected_type:
409 |                 valid_topics.append(topic_path)
410 | 
411 |         # Populate the combo-box
412 |         if self._default_topic is not None and self._default_topic not in valid_topics:
413 |             self._widget.topic_combo_box.addItem(self._default_topic)
414 |         self._widget.topic_combo_box.addItems(valid_topics)
415 |         if self._default_topic is None and self._saved_settings_topic is not None:
416 |             self._widget.topic_combo_box.addItem(self._saved_settings_topic)
417 |         if self._default_topic is None and not valid_topics and self._saved_settings_topic is None:
418 |             self._widget.topic_combo_box.addItem(RosBehaviourTree._empty_topic)
419 |             
420 |         # Initialise it with a topic, priority given to:
421 |         #  1) command line choice, i.e. self._default_topic whether
422 |         #     regardless of whether it exists or not
423 |         #  2) the valid topic if only one exists
424 |         #  3) the valid topic that is also in saved settings if more than one exists
425 |         #  4) the first valid topic in the list
426 |         #  5) the saved topic if no valid topics exist
427 |         #  6) the empty topic if no valid topics exist and no saved topic exists
428 |         # For each choice except 6), set as current with a subscriber.
429 |         if self.current_topic is None:
430 |             if self._default_topic is not None:
431 |                 topic = self._default_topic
432 |             elif len(valid_topics) == 1:
433 |                 topic = valid_topics[0]
434 |             elif valid_topics:
435 |                 if self._saved_settings_topic in valid_topics:
436 |                     topic = self._saved_settings_topic
437 |                 else:
438 |                     topic = valid_topics[0]
439 |             elif self._saved_settings_topic is not None:
440 |                 topic = self._saved_settings_topic
441 |             else:
442 |                 topic = None
443 |             if topic is not None:
444 |                 for index in range(self._widget.topic_combo_box.count()):
445 |                     if topic == self._widget.topic_combo_box.itemText(index):
446 |                         self._widget.topic_combo_box.setCurrentIndex(index)
447 |                         self._choose_topic(topic)
448 |                         break
449 |             return
450 | 
451 |     def _set_timeline_buttons(self, first_snapshot=None, previous_snapshot=None, next_snapshot=None, last_snapshot=None):
452 |         """
453 |         Allows timeline buttons to be enabled and disabled.
454 |         """
455 |         if first_snapshot is not None:
456 |             self._widget.first_tool_button.setEnabled(first_snapshot)
457 |         if previous_snapshot is not None:
458 |             self._widget.previous_tool_button.setEnabled(previous_snapshot)
459 |         if next_snapshot is not None:
460 |             self._widget.next_tool_button.setEnabled(next_snapshot)
461 |         if last_snapshot is not None:
462 |             self._widget.last_tool_button.setEnabled(last_snapshot)
463 | 
464 |     def _play(self):
465 |         """
466 |         Start a timer which will automatically call the next function every time its
467 |         duration is up. Only works if the current message is not the final one.
468 |         """
469 |         if not self._play_timer:
470 |             self._play_timer = rospy.Timer(rospy.Duration(1), self._timer_next)
471 | 
472 |     def _timer_next(self, timer):
473 |         """
474 |         Helper function for the timer so that it can call the next function without
475 |         breaking.
476 |         """
477 |         self._next()
478 | 
479 |     def _stop(self):
480 |         """Stop the play timer, if it exists.
481 |         """
482 |         if self._play_timer:
483 |             self._play_timer.shutdown()
484 |             self._play_timer = None
485 | 
486 |     def _first(self):
487 |         """Navigate to the first message. Activates the next and last buttons, disables
488 |         first and previous, and refreshes the view. Also changes the state to be
489 |         browsing the timeline.
490 | 
491 |         """
492 |         self._timeline.navigate_start()
493 | 
494 |         self._set_timeline_buttons(first_snapshot=False, previous_snapshot=False, next_snapshot=True, last_snapshot=True)
495 |         self._refresh_view.emit()
496 |         self._browsing_timeline = True
497 | 
498 |     def _previous(self):
499 |         """Navigate to the previous message. Activates the next and last buttons, and
500 |         refreshes the view. If the current message is the second message, then
501 |         the first and previous buttons are disabled. Changes the state to be
502 |         browsing the timeline.
503 |         """
504 |         # if already at the beginning, do nothing
505 |         if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp():
506 |             return
507 | 
508 |         # otherwise, go to the previous message
509 |         self._timeline.navigate_previous()
510 |         # now browsing the timeline
511 |         self._browsing_timeline = True
512 |         self._set_timeline_buttons(last_snapshot=True, next_snapshot=True)
513 |         # if now at the beginning, disable timeline buttons.
514 |         if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp():
515 |             self._set_timeline_buttons(next_snapshot=False, last_snapshot=False)
516 | 
517 |         self._refresh_view.emit()
518 | 
519 |     def _last(self):
520 |         """Navigate to the last message. Activates the first and previous buttons,
521 |         disables next and last, and refreshes the view. The user is no longer
522 |         browsing the timeline after this is called.
523 | 
524 |         """
525 |         self._timeline.navigate_end()
526 | 
527 |         self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True, next_snapshot=False, last_snapshot=False)
528 |         self._refresh_view.emit()
529 |         self._browsing_timeline = False
530 |         self._new_messages = 0
531 | 
532 |     def _next(self):
533 |         """Navigate to the next message. Activates the first and previous buttons. If
534 |         the current message is the second from last, disables the next and last
535 |         buttons, and stops browsing the timeline.
536 | 
537 |         """
538 |         # if already at the end, do nothing
539 |         if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp():
540 |             return
541 | 
542 |         # otherwise, go to the next message
543 |         self._timeline.navigate_next()
544 |         self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True)
545 |         # if now at the end, disable timeline buttons and shutdown the play timer if active
546 |         if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp():
547 |             self._set_timeline_buttons(next_snapshot=False, last_snapshot=False)
548 |             self._browsing_timeline = False
549 |             if self._play_timer:
550 |                 self._play_timer.shutdown()
551 | 
552 |         self._refresh_view.emit()
553 | 
554 |     def save_settings(self, plugin_settings, instance_settings):
555 |         instance_settings.set_value('visibility_level', self.visibility_level)
556 |         instance_settings.set_value('auto_fit_graph_check_box_state',
557 |                                     self._widget.auto_fit_graph_check_box.isChecked())
558 |         instance_settings.set_value('highlight_connections_check_box_state',
559 |                                     self._widget.highlight_connections_check_box.isChecked())
560 |         combo_text = self._widget.topic_combo_box.currentText()
561 |         if combo_text not in [self._empty_topic, self._unselected_topic]:
562 |             instance_settings.set_value('combo_box_subscribed_topic', combo_text)
563 | 
564 |     def restore_settings(self, plugin_settings, instance_settings):
565 |         try:
566 |             self._widget.auto_fit_graph_check_box.setChecked(
567 |                 instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
568 |             self._widget.highlight_connections_check_box.setChecked(
569 |                 instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
570 |             self._saved_settings_topic = instance_settings.value('combo_box_subscribed_topic', None)
571 |             saved_visibility_level = instance_settings.value('visibility_level', 1)
572 |         except TypeError as e:
573 |             self._widget.auto_fit_graph_check_box.setChecked(True)
574 |             self._widget.highlight_connections_check_box.setChecked(True)
575 |             self._saved_settings_topic = None
576 |             saved_visibility_level = 1
577 |             rospy.logerr("Rqt PyTrees: incompatible qt app configuration found, try removing ~/.config/ros.org/rqt_gui.ini")
578 |             rospy.logerr("Rqt PyTrees: %s" % e)
579 |         self._widget.visibility_level_combo_box.setCurrentIndex(visibility.saved_setting_to_combo_index[saved_visibility_level])
580 |         self.initialized = True
581 |         self._update_combo_topics()
582 |         self._refresh_tree_graph()
583 | 
584 |     def _refresh_tree_graph(self):
585 |         """Refresh the graph view by regenerating the dotcode from the current message.
586 | 
587 |         """
588 |         if not self.initialized:
589 |             return
590 |         self._update_graph_view(self._generate_dotcode(self.get_current_message()))
591 | 
592 |     def _generate_dotcode(self, message):
593 |         """
594 |         Get the dotcode for the given message, checking the cache for dotcode that
595 |         was previously generated, and adding to the cache if it wasn't there.
596 |         Cache replaces LRU.
597 | 
598 |         Mostly stolen from rqt_bag.MessageLoaderThread
599 | 
600 |         :param py_trees_msgs.BehavoiurTree message
601 |         """
602 |         if message is None:
603 |             return ""
604 | 
605 |         #######################################################
606 |         # Get the tip, from the perspective of the root
607 |         #######################################################
608 |         # this is pretty inefficient, and ignores caching
609 |         tip_id = None
610 |         self._tip_message = None
611 |         # reverse behaviour list - construction puts the root at the end (with
612 |         # visitor, at least)
613 |         for behaviour in reversed(message.behaviours):
614 |             # root has empty parent ID
615 |             if str(behaviour.parent_id) == str(uuid_msgs.UniqueID()):
616 |                 # parent is the root behaviour, so
617 |                 tip_id = behaviour.tip_id
618 | 
619 |         # Run through the behaviours and do a couple of things:
620 |         #  - get the tip
621 |         #  - protect against feedback messages with quotes (https://bitbucket.org/yujinrobot/gopher_crazy_hospital/issues/72/rqt_py_trees-fails-to-display-tree)
622 |         if self._tip_message is None:
623 |             for behaviour in message.behaviours:
624 |                 if str(behaviour.own_id) == str(tip_id):
625 |                     self._tip_message = behaviour.message
626 |                 if '"' in behaviour.message:
627 |                     print("%s" % termcolor.colored('[ERROR] found double quotes in the feedback message [%s]' % behaviour.message, 'red'))
628 |                     behaviour.message = behaviour.message.replace('"', '')
629 |                     print("%s" % termcolor.colored('[ERROR] stripped to stop from crashing, but do catch the culprit! [%s]' % behaviour.message, 'red'))
630 | 
631 |         key = str(message.header.stamp)  # stamps are unique
632 |         if key in self._dotcode_cache:
633 |             return self._dotcode_cache[key]
634 | 
635 |         force_refresh = self._force_refresh
636 |         self._force_refresh = False
637 | 
638 |         visible_behaviours = visibility.filter_behaviours_by_visibility_level(message.behaviours, self.visibility_level)
639 | 
640 |         # cache miss
641 |         dotcode = self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
642 |                                                           behaviours=visible_behaviours,
643 |                                                           timestamp=message.header.stamp,
644 |                                                           force_refresh=force_refresh
645 |                                                           )
646 |         self._dotcode_cache[key] = dotcode
647 |         self._dotcode_cache_keys.append(key)
648 | 
649 |         if len(self._dotcode_cache) > self._dotcode_cache_capacity:
650 |             oldest = self._dotcode_cache_keys[0]
651 |             del self._dotcode_cache[oldest]
652 |             self._dotcode_cache_keys.remove(oldest)
653 | 
654 |         return dotcode
655 | 
656 |     def _update_graph_view(self, dotcode):
657 |         if dotcode == self._current_dotcode:
658 |             return
659 |         self._current_dotcode = dotcode
660 |         self._redraw_graph_view()
661 | 
662 |     def _redraw_graph_view(self):
663 |         key = str(self.get_current_message().header.stamp)
664 |         if key in self._scene_cache:
665 |             new_scene = self._scene_cache[key]
666 |         else:  # cache miss
667 |             new_scene = QGraphicsScene()
668 |             new_scene.setBackgroundBrush(Qt.white)
669 | 
670 |             if self._widget.highlight_connections_check_box.isChecked():
671 |                 highlight_level = 3
672 |             else:
673 |                 highlight_level = 1
674 | 
675 |             # (nodes, edges) = self.dot_to_qt.graph_to_qt_items(self.dotcode_generator.graph,
676 |             #                                                  highlight_level)
677 |             # this function is very expensive
678 |             (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
679 |                                                                 highlight_level)
680 | 
681 |             for node_item in iter(nodes.values()):
682 |                 new_scene.addItem(node_item)
683 |             for edge_items in iter(edges.values()):
684 |                 for edge_item in edge_items:
685 |                     edge_item.add_to_scene(new_scene)
686 | 
687 |             new_scene.setSceneRect(new_scene.itemsBoundingRect())
688 | 
689 |             # put the scene in the cache
690 |             self._scene_cache[key] = new_scene
691 |             self._scene_cache_keys.append(key)
692 | 
693 |             if len(self._scene_cache) > self._scene_cache_capacity:
694 |                 oldest = self._scene_cache_keys[0]
695 |                 del self._scene_cache[oldest]
696 |                 self._scene_cache_keys.remove(oldest)
697 | 
698 |         # after construction, set the scene and fit to the view
699 |         self._scene = new_scene
700 | 
701 |         self._widget.graphics_view.setScene(self._scene)
702 |         self._widget.message_label.setText(self._tip_message)
703 | 
704 |         if self._widget.auto_fit_graph_check_box.isChecked():
705 |             self._fit_in_view()
706 | 
707 |     def _resize_event(self, event):
708 |         """Activated when the window is resized. Will re-fit the behaviour tree in the
709 |         window, and update the size of the timeline scene rectangle so that it
710 |         is correctly drawn.
711 | 
712 |         """
713 |         self._fit_in_view()
714 |         if self._timeline:
715 |             self._timeline.setSceneRect(0, 0, self._widget.timeline_graphics_view.width() - 2, max(self._widget.timeline_graphics_view.height() - 2, self._timeline._timeline_frame._history_bottom))
716 | 
717 |     def timeline_changed(self):
718 |         """Should be called whenever the timeline changes. At the moment this is only
719 |         used to ensure that the first and previous buttons are correctly
720 |         disabled when a new message coming in on the timeline pushes the
721 |         playhead to be at the first message
722 | 
723 |         """
724 |         if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp():
725 |             self._set_timeline_buttons(first_snapshot=False, previous_snapshot=False)
726 |         else:
727 |             self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True)
728 | 
729 |     def message_changed(self):
730 |         """
731 |         This function should be called when the message being viewed changes. Will
732 |         change the current message and update the view. Also ensures that the
733 |         timeline buttons are correctly set for the current position of the
734 |         playhead on the timeline.
735 |         """
736 |         if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp():
737 |             self._set_timeline_buttons(last_snapshot=False, next_snapshot=False)
738 |         else:
739 |             self._set_timeline_buttons(last_snapshot=True, next_snapshot=True)
740 | 
741 |         if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp():
742 |             self._set_timeline_buttons(first_snapshot=False, previous_snapshot=False)
743 |         else:
744 |             self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True)
745 | 
746 |         self._refresh_view.emit()
747 | 
748 |     def message_cleared(self):
749 |         """
750 |         This function should be called when the message being viewed was cleared.
751 |         Currently no situation where this happens?
752 |         """
753 |         pass
754 | 
755 |     def no_right_click_press_event(self, func):
756 |         """Decorator for ignoring right click events on mouse press
757 |         """
758 |         @functools.wraps(func)
759 |         def wrapper(event):
760 |             if event.type() == QEvent.MouseButtonPress and event.button() == Qt.RightButton:
761 |                 event.ignore()
762 |             else:
763 |                 func(event)
764 |         return wrapper
765 | 
766 |     def _set_dynamic_timeline(self):
767 |         """
768 |         Set the timeline to a dynamic timeline, listening to messages on the topic
769 |         selected in the combo box.
770 |         """
771 |         self._timeline = DynamicTimeline(self, publish_clock=False)
772 |         # connect timeline events so that the timeline will update when events happen
773 |         self._widget.timeline_graphics_view.mousePressEvent = self.no_right_click_press_event(self._timeline.on_mouse_down)
774 |         self._widget.timeline_graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up
775 |         self._widget.timeline_graphics_view.mouseMoveEvent = self._timeline.on_mouse_move
776 |         self._widget.timeline_graphics_view.wheelEvent = self._timeline.on_mousewheel
777 |         self._widget.timeline_graphics_view.setScene(self._timeline)
778 | 
779 |         # Don't show scrollbars - the timeline adjusts to the size of the view
780 |         self._widget.timeline_graphics_view.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
781 |         self._widget.timeline_graphics_view.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
782 |         # Send a resize event so that the timeline knows the size of the view it's in
783 |         self._resize_event(None)
784 | 
785 |         self._timeline.add_topic(self.current_topic, py_trees_msgs.BehaviourTree)
786 | 
787 |         # Create a listener for the timeline which will call the emit function
788 |         # on the given signals when the message being viewed changes or is
789 |         # cleared. The message being viewed changing generally happens when the
790 |         # user moves the slider around.
791 |         self._timeline_listener = DynamicTimelineListener(self._timeline, self.current_topic, self._message_changed, self._message_cleared)
792 |         # Need to add a listener to make sure that we can get information about
793 |         # messages that are on the topic that we're interested in.
794 |         self._timeline.add_listener(self.current_topic, self._timeline_listener)
795 | 
796 |         self._timeline.navigate_end()
797 |         self._timeline._redraw_timeline(None)
798 |         self._timeline.timeline_updated.connect(self.timeline_changed)
799 | 
800 |     def _set_bag_timeline(self, bag):
801 |         """Set the timeline of this object to a bag timeline, hooking the graphics view
802 |         into mouse and wheel functions of the timeline.
803 | 
804 |         """
805 |         self._timeline = BagTimeline(self, publish_clock=False)
806 |         # connect timeline events so that the timeline will update when events happen
807 |         self._widget.timeline_graphics_view.mousePressEvent = self.no_right_click_press_event(self._timeline.on_mouse_down)
808 |         self._widget.timeline_graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up
809 |         self._widget.timeline_graphics_view.mouseMoveEvent = self._timeline.on_mouse_move
810 |         self._widget.timeline_graphics_view.wheelEvent = self._timeline.on_mousewheel
811 |         self._widget.timeline_graphics_view.setScene(self._timeline)
812 | 
813 |         # Don't show scrollbars - the timeline adjusts to the size of the view
814 |         self._widget.timeline_graphics_view.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
815 |         self._widget.timeline_graphics_view.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
816 |         # Send a resize event so that the timeline knows the size of the view it's in
817 |         self._resize_event(None)
818 | 
819 |         self._timeline.add_bag(bag)
820 |         # Create a listener for the timeline which will call the emit function
821 |         # on the given signals when the message being viewed changes or is
822 |         # cleared. The message being viewed changing generally happens when the
823 |         # user moves the slider around.
824 |         self._timeline_listener = TimelineListener(self._timeline, self.current_topic, self._message_changed, self._message_cleared)
825 |         # Need to add a listener to make sure that we can get information about
826 |         # messages that are on the topic that we're interested in.
827 |         self._timeline.add_listener(self.current_topic, self._timeline_listener)
828 |         # Go to the first message in the timeline of the bag.
829 |         self._timeline.navigate_start()
830 | 
831 |     def _load_bag(self, file_name=None):
832 |         """Load a bag from file. If no file name is given, a dialogue will pop up and
833 |         the user will be asked to select a file. If the bag file selected
834 |         doesn't have any valid topic, nothing will happen. If there are valid
835 |         topics, we load the bag and add a timeline for managing it.
836 | 
837 |         """
838 |         if file_name is None:
839 |             file_name, _ = QFileDialog.getOpenFileName(
840 |                 self._widget,
841 |                 self.tr('Open trees from bag file'),
842 |                 None,
843 |                 self.tr('ROS bag (*.bag)'))
844 |         if file_name is None or file_name == "":
845 |             return
846 | 
847 |         rospy.loginfo("Reading bag from {0}".format(file_name))
848 |         bag = rosbag.Bag(file_name, 'r')
849 |         # ugh...
850 |         topics = list(bag.get_type_and_topic_info()[1].keys())
851 |         types = []
852 |         for i in range(0, len(bag.get_type_and_topic_info()[1].values())):
853 |             types.append(list(bag.get_type_and_topic_info()[1].values())[i][0])
854 | 
855 |         tree_topics = []  # only look at the first matching topic
856 |         for ind, tp in enumerate(types):
857 |             if tp == 'py_trees_msgs/BehaviourTree':
858 |                 tree_topics.append(topics[ind])
859 | 
860 |         if len(tree_topics) == 0:
861 |             rospy.logerr('Requested bag did not contain any valid topics.')
862 |             return
863 | 
864 |         self.message_list = []
865 |         self._viewing_bag = True
866 |         rospy.loginfo('Reading behaviour trees from topic {0}'.format(tree_topics[0]))
867 |         for unused_topic, msg, unused_t in bag.read_messages(topics=[tree_topics[0]]):
868 |             self.message_list.append(msg)
869 | 
870 |         self.current_topic = tree_topics[0]
871 |         self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True, next_snapshot=False, last_snapshot=False)
872 |         self._set_bag_timeline(bag)
873 |         self._refresh_view.emit()
874 | 
875 |     def _load_dot(self, file_name=None):
876 |         if file_name is None:
877 |             file_name, _ = QFileDialog.getOpenFileName(
878 |                 self._widget,
879 |                 self.tr('Open tree from DOT file'),
880 |                 None,
881 |                 self.tr('DOT graph (*.dot)'))
882 |             if file_name is None or file_name == '':
883 |                 return
884 | 
885 |         try:
886 |             fhandle = open(file_name, 'rb')
887 |             dotcode = fhandle.read()
888 |             fhandle.close()
889 |         except IOError:
890 |             return
891 |         self._update_graph_view(dotcode)
892 | 
893 |     def _fit_in_view(self):
894 |         self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
895 |                                              Qt.KeepAspectRatio)
896 | 
897 |     def _save_dot(self):
898 |         file_name, _ = QFileDialog.getSaveFileName(self._widget,
899 |                                                    self.tr('Save as DOT'),
900 |                                                    'frames.dot',
901 |                                                    self.tr('DOT graph (*.dot)'))
902 |         if file_name is None or file_name == '':
903 |             return
904 | 
905 |         dot_file = QFile(file_name)
906 |         if not dot_file.open(QIODevice.WriteOnly | QIODevice.Text):
907 |             return
908 | 
909 |         dot_file.write(self._current_dotcode)
910 |         dot_file.close()
911 | 
912 |     def _save_svg(self):
913 |         file_name, _ = QFileDialog.getSaveFileName(
914 |             self._widget,
915 |             self.tr('Save as SVG'),
916 |             'frames.svg',
917 |             self.tr('Scalable Vector Graphic (*.svg)'))
918 |         if file_name is None or file_name == '':
919 |             return
920 | 
921 |         generator = QSvgGenerator()
922 |         generator.setFileName(file_name)
923 |         generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
924 | 
925 |         painter = QPainter(generator)
926 |         painter.setRenderHint(QPainter.Antialiasing)
927 |         self._scene.render(painter)
928 |         painter.end()
929 | 
930 |     def _save_image(self):
931 |         file_name, _ = QFileDialog.getSaveFileName(
932 |             self._widget,
933 |             self.tr('Save as image'),
934 |             'frames.png',
935 |             self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
936 |         if file_name is None or file_name == '':
937 |             return
938 | 
939 |         img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
940 |                      QImage.Format_ARGB32_Premultiplied)
941 |         painter = QPainter(img)
942 |         painter.setRenderHint(QPainter.Antialiasing)
943 |         self._scene.render(painter)
944 |         painter.end()
945 |         img.save(file_name)
946 | 


--------------------------------------------------------------------------------