├── .devcontainer ├── Dockerfile ├── README.md └── devcontainer.json ├── .github └── workflows │ ├── pre-merge.yaml │ └── push-ci-image.yaml ├── .gitignore ├── .readthedocs.yaml ├── .settings └── org.eclipse.core.resources.prefs ├── CHANGELOG.rst ├── LICENSE ├── README.md ├── docs ├── .gitignore ├── Makefile ├── images │ ├── blackboard-watcher.gif │ ├── tree-watcher.gif │ ├── trees.png │ └── trees_dot_graph.png ├── requirements.txt ├── sources │ ├── about.rst │ ├── changelog.rst │ ├── conf.py │ ├── dot │ │ ├── demo-exchange.dot │ │ ├── demo_tree.dot │ │ ├── tutorial-eight-scan.dot │ │ ├── tutorial-eight.dot │ │ ├── tutorial-five-guard.dot │ │ ├── tutorial-five-preempt.dot │ │ ├── tutorial-five.dot │ │ ├── tutorial-one.dot │ │ ├── tutorial-seven.dot │ │ ├── tutorial-six-context-switch.dot │ │ ├── tutorial-six.dot │ │ └── tutorial-two.dot │ ├── features.rst │ ├── images │ │ ├── blackboard-watcher.gif │ │ ├── tree-watcher.gif │ │ ├── trees.png │ │ └── trees_dot_graph.png │ ├── index.rst │ ├── modules.rst │ └── weblinks.rst └── venv.bash ├── package.xml ├── py_trees_ros ├── __init__.py ├── action_clients.py ├── actions.py ├── battery.py ├── blackboard.py ├── conversions.py ├── exceptions.py ├── mock │ ├── __init__.py │ ├── actions.py │ ├── dock.py │ ├── services.py │ └── set_bool.py ├── programs │ ├── __init__.py │ ├── blackboard_watcher.py │ ├── echo.py │ ├── multi_talker.py │ └── tree_watcher.py ├── publishers.py ├── service_clients.py ├── subscribers.py ├── transforms.py ├── trees.py ├── utilities.py ├── version.py └── visitors.py ├── resources └── py_trees_ros ├── setup.cfg ├── setup.py └── tests ├── README.md ├── __init__.py ├── test_action_client.py ├── test_blackboard.py ├── test_expand_topic_name.py ├── test_publishers.py ├── test_service_client.py ├── test_subscribers.py └── test_transforms.py /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Base 3 | # - https://hub.docker.com/_/ros/ 4 | ################################################################################ 5 | 6 | ARG ROS_DISTRO=foxy 7 | 8 | FROM ros:${ROS_DISTRO} 9 | 10 | ARG NAME=ros-${ROS_DISTRO} 11 | 12 | ################################################################################ 13 | # Updates 14 | ################################################################################ 15 | 16 | # TODO: Delete the list of debs. These are merely the list 17 | # required by rosdep for the py-trees set of repos on foxy 18 | # and this would be better handled by rosdep itself 19 | # so it can gracefully transition across distros. 20 | # 21 | # NB: Installing debs here for now to expedite testing of the 22 | # devcontainer configuration (esp. for qt, X11). 23 | # 24 | RUN apt-get update && apt-get install -y --no-install-recommends \ 25 | # poetry 26 | curl \ 27 | python3-dev \ 28 | # docs 29 | python3-venv \ 30 | # development 31 | bash \ 32 | bash-completion \ 33 | ca-certificates \ 34 | git \ 35 | less \ 36 | make \ 37 | ssh \ 38 | vim \ 39 | wget \ 40 | # Other 41 | qttools5-dev-tools 42 | # Still need these? 43 | # libpyside2-dev \ 44 | # libshiboken2-dev \ 45 | # pyqt5-dev \ 46 | # python3-pyqt5.qtsvg \ 47 | # python3-pyside2.qtsvg \ 48 | # python3-sip-dev \ 49 | # shiboken2 \ 50 | # python3-pyqt5.qtwebengine \ 51 | # python3-mypy 52 | 53 | #################### 54 | # OpenGL 55 | #################### 56 | # mesa-utils : glxgears + gl libs (libgl# libglvnd#, libglx#) 57 | # egl: not needed (libegl1, libgles2) 58 | # vulkan: not needed 59 | RUN apt-get install -y --no-install-recommends \ 60 | mesa-utils 61 | 62 | ################################################################################ 63 | # NVIDIA 64 | ################################################################################ 65 | 66 | ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all} 67 | ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics,display,video,utility,compute 68 | 69 | ################################################################################ 70 | # Login Shells for Debugging & Development 71 | ################################################################################ 72 | 73 | # In a login shell (below), the PATH env doesn't survive, configure it at ground zero 74 | # RUN echo "export PATH=${POETRY_HOME}/bin:${PATH}" >> /etc/profile 75 | ENV TERM xterm-256color 76 | ENTRYPOINT ["/bin/bash", "--login", "-i"] 77 | 78 | ################################################################################ 79 | # Development with a user, e.g. for vscode devcontainers 80 | ################################################################################ 81 | 82 | ARG USERNAME=zen 83 | ARG USER_UID=1000 84 | ARG USER_GID=${USER_UID} 85 | 86 | RUN groupadd --gid $USER_GID $USERNAME && \ 87 | useradd --uid $USER_UID --gid $USER_GID -s "/bin/bash" -m $USERNAME && \ 88 | apt-get install -y sudo && \ 89 | echo "${USERNAME} ALL=NOPASSWD: ALL" > /etc/sudoers.d/${USERNAME} && \ 90 | chmod 0440 /etc/sudoers.d/${USERNAME} 91 | RUN echo "export PS1='\[\033[01;36m\](docker)\[\033[00m\] \[\033[01;32m\]\u@${NAME}\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '" >> /home/${USERNAME}/.bashrc && \ 92 | echo "alias ll='ls --color=auto -alFNh'" >> /home/${USERNAME}/.bashrc && \ 93 | echo "alias ls='ls --color=auto -Nh'" >> /home/${USERNAME}/.bashrc 94 | 95 | USER zen 96 | RUN rosdep update 97 | USER root 98 | 99 | ################################################################################ 100 | # Debugging with root 101 | ################################################################################ 102 | 103 | RUN echo "export PS1='\[\033[01;36m\](docker)\[\033[00m\] \[\033[01;32m\]\u@${NAME}\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '" >> ${HOME}/.bashrc && \ 104 | echo "alias ll='ls --color=auto -alFNh'" >> ${HOME}/.bashrc && \ 105 | echo "alias ls='ls --color=auto -Nh'" >> ${HOME}/.bashrc 106 | -------------------------------------------------------------------------------- /.devcontainer/README.md: -------------------------------------------------------------------------------- 1 | # Development Environment 2 | 3 | ## Requirements 4 | 5 | This assumes the host is running an XOrg server (e.g. Ubuntu). This is relayed to the 6 | devcontainer to run the qt viwers. You may need a different devcontainer configuration if running, 7 | e.g. a Wayland server. 8 | 9 | ## Sources 10 | 11 | ``` 12 | # Fetch sources 13 | $ mkdir src 14 | $ wget -O - https://raw.githubusercontent.com/stonier/repos_index/devel/foxy/py_trees.repos | vcs import ./src 15 | 16 | # Open VSCode 17 | $ code ./src/py_trees_ros 18 | ``` 19 | 20 | ## VSCode DevContainer 21 | 22 | At this point you can use VSCode to "Re-open project in container" -> this will bring everything from 23 | the root of the colcon workspace (i.e. below `./src`) into both the devcontainer and the vscode UI. 24 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "py_trees_ros", 3 | 4 | "build": { 5 | "dockerfile": "./Dockerfile", 6 | "args": { 7 | "NAME": "py_trees_ros", 8 | "ROS_DISTRO": "humble" 9 | }, 10 | "context": ".." 11 | }, 12 | "remoteUser": "zen", 13 | "workspaceMount": "source=${localWorkspaceFolder}/../..,target=/workspace,type=bind", 14 | "workspaceFolder": "/workspace", 15 | // Qt Configuration - assumes the host has an Xorg server, you 16 | // may need a different configuration for e.g. Wayland. 17 | "containerEnv": { 18 | "DISPLAY": "${localEnv:DISPLAY}", 19 | "QT_DEBUG_PLUGINS": "1" 20 | }, 21 | "customizations": { 22 | "vscode": { 23 | "extensions": [ 24 | "bierner.github-markdown-preview", 25 | "bierner.markdown-preview-github-styles", 26 | "bungcip.better-toml", 27 | "streetsidesoftware.code-spell-checker", 28 | "ms-python.python", 29 | "omnilib.ufmt", 30 | "tht13.rst-vscode" 31 | ] 32 | } 33 | }, 34 | "mounts": [ 35 | { 36 | "source": "/tmp/.X11-unix", 37 | "target": "/tmp/.X11-unix", 38 | "type": "bind" 39 | } 40 | ], 41 | "runArgs": [ 42 | "--runtime=nvidia", 43 | "--gpus", 44 | "all", 45 | "--network", 46 | "host" 47 | ] 48 | } 49 | -------------------------------------------------------------------------------- /.github/workflows/pre-merge.yaml: -------------------------------------------------------------------------------- 1 | name: pre-merge 2 | 3 | env: 4 | REGISTRY: ghcr.io 5 | IMAGE_NAME: ${{ github.repository }} 6 | ROS_DISTRO: rolling 7 | 8 | on: 9 | pull_request: 10 | workflow_dispatch: 11 | 12 | jobs: 13 | pre-merge: 14 | runs-on: ubuntu-22.04 15 | container: 16 | image: ghcr.io/${{ github.repository }}-ci:rolling 17 | credentials: 18 | username: ${{ github.actor }} 19 | password: ${{ secrets.GITHUB_TOKEN }} 20 | 21 | steps: 22 | - name: Checkout py_trees 23 | uses: actions/checkout@v3 24 | with: 25 | repository: splintered-reality/py_trees 26 | path: src/py_trees 27 | - name: Checkout py_trees_ros_interfaces 28 | uses: actions/checkout@v3 29 | with: 30 | repository: splintered-reality/py_trees_ros_interfaces 31 | path: src/py_trees_ros_interfaces 32 | - name: Checkout py_trees_ros 33 | uses: actions/checkout@v3 34 | with: 35 | path: src/py_trees_ros 36 | 37 | - name: Dependencies 38 | run: | 39 | rosdep update 40 | rosdep install --from-paths src --rosdistro ${{ env.ROS_DISTRO }} --ignore-src -r -y 41 | 42 | - name: Build 43 | shell: bash 44 | run: | 45 | . /opt/ros/${{ env.ROS_DISTRO }}/setup.bash 46 | VERBOSE=1 colcon build --symlink-install --event-handlers console_direct+ --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_VERBOSE_MAKEFILE:=ON 47 | 48 | - name: Tests 49 | shell: bash 50 | run: | 51 | . install/setup.bash 52 | cd src/py_trees_ros/tests && pytest-3 -s . 53 | -------------------------------------------------------------------------------- /.github/workflows/push-ci-image.yaml: -------------------------------------------------------------------------------- 1 | name: push-ci-image 2 | 3 | env: 4 | REGISTRY: ghcr.io 5 | IMAGE_NAME: ${{ github.repository }}-ci 6 | 7 | on: 8 | workflow_dispatch: 9 | inputs: 10 | ros_distro: 11 | description: "humble, rolling, ..." 12 | required: false 13 | default: "rolling" 14 | 15 | jobs: 16 | push-ci-image: 17 | runs-on: ubuntu-22.04 18 | permissions: 19 | contents: read 20 | packages: write 21 | steps: 22 | - name: Checkout 23 | uses: actions/checkout@v3 24 | - name: Login to GCR 25 | uses: docker/login-action@v2 26 | with: 27 | registry: ghcr.io 28 | username: ${{ github.actor }} 29 | password: ${{ secrets.GITHUB_TOKEN }} 30 | - name: Metadata 31 | id: meta 32 | uses: docker/metadata-action@v4 33 | with: 34 | images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} 35 | - name: Echo 36 | run: | 37 | echo "USER: ${{ github.actor }}" 38 | echo "REPOSITORY: ${{ github.repository }}" 39 | echo "ROS_DISTRO: ${{ github.event.inputs.ros_distro }}" 40 | echo "TAGS: ${{ steps.meta.outputs.tags }}" 41 | echo "LABELS: ${{ steps.meta.outputs.labels }}" 42 | - name: Image - ${{ github.event.inputs.ros_distro }} 43 | uses: docker/build-push-action@v3 44 | with: 45 | file: ./.devcontainer/Dockerfile 46 | push: true 47 | build-args: | 48 | ROS_DISTRO=${{ github.event.inputs.ros_distro }} 49 | tags: | 50 | ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}:${{ github.event.inputs.ros_distro }} 51 | labels: ${{ steps.meta.outputs.labels }} 52 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | py_trees_ros.egg-info 2 | */doc/html 3 | */doc/manifest.yaml 4 | *.pyc 5 | *.png 6 | *.svg 7 | build 8 | dist 9 | deb_dist 10 | py_trees*.tar.gz 11 | *~ 12 | \#* 13 | .#* 14 | .cache 15 | .eggs 16 | *.md.html 17 | 18 | # Generated files (when debugging) 19 | py_trees_ros/resources/*.py 20 | html 21 | 22 | scripts 23 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yaml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | 8 | # Set the version of Python and other tools you might need 9 | build: 10 | os: ubuntu-22.04 11 | tools: 12 | python: "3.10" 13 | apt_packages: 14 | - graphviz 15 | 16 | # Build documentation in the docs/ directory with Sphinx 17 | sphinx: 18 | configuration: docs/sources/conf.py 19 | fail_on_warning: true 20 | 21 | # If using Sphinx, optionally build your docs in additional formats such as PDF 22 | # formats: 23 | # - pdf 24 | 25 | # Optionally declare the Python requirements required to build your docs 26 | python: 27 | install: 28 | - requirements: docs/requirements.txt 29 | -------------------------------------------------------------------------------- /.settings/org.eclipse.core.resources.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | encoding//doc/conf.py=utf-8 3 | encoding//py_trees_ros/action_clients.py=utf-8 4 | encoding//py_trees_ros/actions.py=utf-8 5 | encoding//py_trees_ros/battery.py=utf-8 6 | encoding//py_trees_ros/blackboard.py=utf-8 7 | encoding//py_trees_ros/conversions.py=utf-8 8 | encoding//py_trees_ros/exceptions.py=utf-8 9 | encoding//py_trees_ros/mock/actions.py=utf-8 10 | encoding//py_trees_ros/mock/dock.py=utf-8 11 | encoding//py_trees_ros/programs/blackboard_watcher.py=utf-8 12 | encoding//py_trees_ros/programs/echo.py=utf-8 13 | encoding//py_trees_ros/programs/multi_talker.py=utf-8 14 | encoding//py_trees_ros/programs/tree_watcher.py=utf-8 15 | encoding//py_trees_ros/publishers.py=utf-8 16 | encoding//py_trees_ros/subscribers.py=utf-8 17 | encoding//py_trees_ros/transforms.py=utf-8 18 | encoding//py_trees_ros/trees.py=utf-8 19 | encoding//py_trees_ros/utilities.py=utf-8 20 | encoding//py_trees_ros/visitors.py=utf-8 21 | encoding//tests/test_action_client.py=utf-8 22 | encoding//tests/test_blackboard.py=utf-8 23 | encoding//tests/test_expand_topic_name.py=utf-8 24 | encoding//tests/test_publishers.py=utf-8 25 | encoding//tests/test_subscribers.py=utf-8 26 | encoding//tests/test_transforms.py=utf-8 27 | encoding/setup.py=utf-8 28 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ========= 2 | Changelog 3 | ========= 4 | 5 | 2.3.0 (2025-01-11) 6 | ------------------ 7 | * [behaviours] Implement Behaviors to Interact with ROS Services (`#215 `_) 8 | * [behaviours] Support setting subscription callback_group (`#220 `_) 9 | * [behaviours] Add keyword arguments to setup 10 | * [vscode] gl capabilities in the devcontainer 11 | * [readme] py-trees-js status for rolling, humble 12 | * [readme] deb install instructions for ros2 13 | * [readme] py-trees-ros build status for rolling, humble 14 | * Contributors: Amal Nanavati, Daniel Stonier, Hervé Audren, fred-labs 15 | 16 | 2.2.2 (2023-02-10) 17 | ------------------ 18 | * [pkg] tf2_ros -> tf2_ros_py, `#204 `_ 19 | 20 | 2.2.1 (2023-02-08) 21 | ------------------ 22 | * [behaviours] eliminate more arg name defaults, `#201 `_ 23 | 24 | 2.2.0 (2023-02-08) 25 | ------------------ 26 | * [actions] pre-merge and push-ci-image actions, `#200 `_ 27 | * [tests] bugfix explicit args and time float errors in tests, `#200 `_ 28 | * [watchers] handle explicit args and memory details, `#196 `_ 29 | * [watchers] bugfix blackboard watcher shutdownon humble, `#194 `_ 30 | * [code] refactor for explicit arguments, `#191 `_ 31 | * [trees] optional node and node name args to setup, `#188 `_ 32 | * [trees] switch deprecating qos policy for shorter versions, `#187 `_ 33 | 34 | 2.1.1 (2021-05-09) 35 | ------------------ 36 | * [trees] publish with/without memory information (supports new selector/sequence modes) 37 | 38 | 2.1.0 (2020-08-02) 39 | ------------------ 40 | * [infra] update for api changes in py_trees 2.1.0 (Chooser no longer supported) 41 | * [infra] update for api changes in ros2 foxy (not breaking changes) 42 | 43 | 2.0.11 (2020-03-23) 44 | ------------------- 45 | * [blackboards] log a one-shot warning instead of exceptions when pickle fails, `#156 `_ 46 | 47 | 2.0.10 (2020-03-06) 48 | ------------------- 49 | * [mock] server bugfixed to accommodate custom result handlers, `#154 `_ 50 | 51 | 2.0.9 (2020-03-05) 52 | ------------------ 53 | * [behaviours] default to periodic checks with warnings for action client timeouts, `#153 `_ 54 | 55 | 2.0.8 (2020-03-04) 56 | ------------------ 57 | * [behaviours] wait_for_server_timeout_sec in action clients, `#152 `_ 58 | * [trees] avoid irregular snapshot streams by avoiding timing jitter, `#151 `_ 59 | 60 | 2.0.7 (2020-02-10) 61 | ------------------ 62 | * [programs] graph discovery timeouts 1.0->2.0s, `#149 `_ 63 | 64 | 2.0.6 (2020-02-06) 65 | ------------------ 66 | * [behaviours] action client goals from blackboard behaviour, `#148 `_ 67 | * [behaviours] publish from blackboard behaviour, `#146 `_ 68 | 69 | 2.0.5 (2020-01-24) 70 | ------------------ 71 | * [serialisation] include parallel policy details 72 | 73 | 2.0.4 (2019-12-30) 74 | ------------------ 75 | * [blackboard] bugfix protection against activity stream not activitated for views, `#140 `_ 76 | * [trees] bugfix toggle for activity client on reconfigure, `#139 `_ 77 | 78 | 2.0.3 (2019-12-26) 79 | ------------------ 80 | * [infra] add a marker file to assist with ros2cli discovery `#138 `_ 81 | 82 | 2.0.2 (2019-12-26) 83 | ------------------ 84 | * [trees] on-demand snapshot stream services, similar to blackboard streams, `#135 `_, `#136 `_ 85 | 86 | 2.0.1 (2019-12-03) 87 | ------------------ 88 | * [trees] periodically publish snapshots, `#131 `_, 89 | * [trees] permit setup visitors, provide a default with timings, `#129 `_ 90 | * [trees] bugfix non-infinite timeout arg getting ignored, `#129 `_ 91 | * [trees] snapshot now publishing tree changed flag and key access info, `#128 `_ 92 | * [utilities] deprecate myargv for rclpy.utilities.remove_ros_args, `#130 `_ 93 | 94 | 2.0.0 (2019-11-20) 95 | ------------------ 96 | * [blackboards] updated pretty printing to differentiate namespace vs attribute access, `#123 `_ 97 | * [blackboards] api updates for namespaced clients, `#122 `_, 98 | * [tests] migrated tests from unittest to pytest 99 | * [transforms] behaviours for writing to and broadcasting from the blackboard, `#121 `_ 100 | * [transforms] add missing mocks and update to latest blackboard api, `#125 `_ 101 | 102 | 1.2.1 (2019-10-08) 103 | ------------------ 104 | * [trees] bugfix KeyError on publication of missing keys, `#118 `_ 105 | * [utilities] a ros myargv stipper, a'la ROS1 style, until something is available upstream 106 | 107 | 1.2.0 (2019-10-02) 108 | ------------------ 109 | * [blackboards] sweeping changes to accomodate the new blackboards with tracking, `#109 `_ 110 | * [backend] ensure tree modifications are published with an updated timestamp, `#100 `_ 111 | * [behaviours] subscriber related behaviours now *require* qos_profile args, `#104 `_ 112 | * [trees] ros parameterisation of the setup timeout, `#101 `_ 113 | * [trees] make use of the new `DisplaySnapshotVisitor`, `#102 `_ 114 | 115 | 1.1.2 (2019-08-10) 116 | ------------------ 117 | * [utilities] permit discovery of multiples with find_topics, `#97 `_ 118 | 119 | 1.1.1 (2019-06-22) 120 | ------------------ 121 | * [tests] add missing tests/__init.py, `#94 `_ 122 | * [infra] add missing ros2topic dependency, `#94 `_ 123 | 124 | 1.1.0 (2019-06-19) 125 | ------------------ 126 | 127 | * [actions] bugfix action client, don't cancel if not RUNNING 128 | * [conversions] bugfix msg_to_behaviour for decorators 129 | * [watchers] bugfix tree-watchers dot-graph to string functionality 130 | * [watchers] bugfix missing tip in deserialised tree-watcher tree 131 | 132 | 1.0.0 (2019-04-28) 133 | ------------------ 134 | 135 | Stripped down and rebuilt for ROS2: 136 | 137 | * [behaviours] the familiar subscriber and action client behaviours 138 | * [blackboard] the exchange, mostly unmodified 139 | * [infra] colcon build environment 140 | * [trees] simpler communications, just one serialised tree snapshot, watchers do the rest 141 | * [watchers] revamped 'blackboard' and new 'tree' watcher 142 | 143 | What's missing: 144 | 145 | * [logging] the basic mechanisms have moved to py_trees, the rosbag implementation is to come 146 | 147 | 0.5.13 (2017-05-28) 148 | ------------------- 149 | * [doc] add many missing packages to satiate autodoc 150 | 151 | 0.5.9 (2017-04-16) 152 | ------------------ 153 | * [doc] add missing rqt-py-trees image 154 | * [infra] bugfix missing install rule for mock sensors script 155 | 156 | 0.5.5 (2017-03-31) 157 | ------------------ 158 | * [infra] missed the py_trees exec dependency, fixed. 159 | 160 | 0.5.4 (2017-03-25) 161 | ------------------ 162 | * [docs] faq added 163 | * [tutorials] 9 - bagging 164 | * [infra] various dependency fixes for tests and autodoc 165 | * [tests] fix broken subscrirber test 166 | 167 | 0.5.3 (2017-03-21) 168 | ------------------ 169 | * [tutorials] 8 - dynamic loading, insertion and execution 170 | * [tutorials] 7 - docking, undocking, cancelling and recovery 171 | 172 | 0.5.2 (2017-03-19) 173 | ------------------ 174 | * [infra] add missing actionlib dependencies 175 | 176 | 0.5.1 (2017-03-19) 177 | ------------------ 178 | * [tutorials] 6 - context switching 179 | * [tutorials] re-insert missing images 180 | 181 | 0.5.0 (2017-02-21) 182 | ------------------ 183 | * [docs] new and shiny index added 184 | * [tutorials] qt dashboard support 185 | * [tutorials] 5 - tree scanning added 186 | * [tutorials] 4 - tree introspection added 187 | * [tutorials] 3 - blackboards added 188 | * [tutorials] 2 - battery low branch added 189 | * [tutorials] 1 - data gathering added 190 | * [mock] a mock robot for tutorials and testing 191 | * [behaviours] action client, battery behaviours added 192 | * [infra] refactoring for kinetic 193 | 194 | Indigo -> Kinetic Changelist 195 | ---------------------------- 196 | 197 | **Py Trees ROS API** 198 | 199 | * **subscribers** 200 | 201 | * py_trees.subscribers.SubscriberHandler -> py_trees_ros.subscribers.Handler 202 | * py_trees.subscribers.CheckSubscriberVariable -> py_trees_ros.subscribers.CheckData 203 | * py_trees.subscribers.WaitForSubscriberData -> py_trees_ros.subscribers.WaitForData 204 | * **conversions** 205 | 206 | * py_trees.converters.convert_type -> py_trees_ros.converters.behaviour_type_to_msg_constant 207 | * py_trees.converters.convert_status -> py_trees_ros.converters.status_enum_to_msg_constant 208 | * py_trees.converters.convert_blackbox -> py_trees_ros.converters.blackbox_enum_to_msg_constant 209 | * **blackboard** 210 | 211 | * py_trees.ros.blackboard -> py_trees_ros.blackboard.Exchange 212 | * ~list_blackboard_variables -> ~get_blackboard_variables 213 | * ~spawn_blackboard_watcher -> ~open_blackboard_watcher 214 | * ~destroy_blackboard_watcher -> ~close_blackboard_watcher 215 | * **visitors** : classes moved from py_trees.trees -> py_trees_ros.visitors 216 | 217 | **Py Trees ROS Msgs API** 218 | 219 | * **blackboard services** 220 | 221 | * py_trees.msgs.srv.BlackboardVariables -> py_trees_msgs.srv.GetBlackboardVariables 222 | * py_trees.msgs.srv.SpawnBlackboardWatcher -> py_trees_msgs.srv.OpenBlackboardWatcher 223 | * py_trees.msgs.srv.DestroyBlackboardWatcher -> py_trees_msgs.srv.CloseBlackboardWatcher 224 | 225 | **Py Trees** 226 | 227 | * **program** : py-trees-render added 228 | * **imposter** : bugfix to permit visitors to the children of a composite original 229 | * **visitors** : py_trees.trees -> py_trees.visitors 230 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | .venv 3 | doctrees 4 | 5 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= -E -vvv 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = ./sources 9 | BUILDDIR = ./build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile clean py_trees_ros.egg-info 16 | 17 | clean: 18 | rm -rf build 19 | 20 | # Catch-all target: route all unknown targets to Sphinx using the new 21 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 22 | %: Makefile 23 | $(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 24 | -------------------------------------------------------------------------------- /docs/images/blackboard-watcher.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/images/blackboard-watcher.gif -------------------------------------------------------------------------------- /docs/images/tree-watcher.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/images/tree-watcher.gif -------------------------------------------------------------------------------- /docs/images/trees.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/images/trees.png -------------------------------------------------------------------------------- /docs/images/trees_dot_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/images/trees_dot_graph.png -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | ############################################################################## 2 | # 3 | # Requirements for sphinx documentation environment. 4 | # ROS dependencies are mocked. 5 | # 6 | # This file is discovered by the root level 7 | # .readthedocs.yaml (rtd build) and docenv.bash (local build) 8 | ############################################################################## 9 | 10 | Sphinx==5.3.0 11 | sphinx-argparse==0.4.0 12 | sphinx_rtd_theme==1.1.1 13 | py_trees>=2.2 14 | # Broken - see notes in conf.py 15 | # sphinx-autodoc-typehints==1.22 16 | -------------------------------------------------------------------------------- /docs/sources/about.rst: -------------------------------------------------------------------------------- 1 | About 2 | ===== 3 | 4 | This package extends `py_trees`_ with behaviours, idioms, a tree manager 5 | (with ROS api) and command line tools for use in a ROS ecosystem. 6 | The documentation in this package is useful for referential purposes only - it 7 | includes the module level api documentation and instructions for the 8 | command line utilities. 9 | 10 | If you are looking for tutorial-like documentation to get started with 11 | behaviour trees, refer to: 12 | 13 | * :ref:`py_trees `: behaviour tree basics 14 | * :ref:`py_trees_ros_tutorials `: using py_trees in ROS 15 | 16 | .. _py_trees: https://github.com/splintered-reality/py_trees 17 | .. _py_trees_ros: https://github.com/splintered-reality/py_trees_ros 18 | .. _py_trees_ros_tutorials: https://github.com/splintered-reality/py_trees_ros_tutorials 19 | 20 | -------------------------------------------------------------------------------- /docs/sources/changelog.rst: -------------------------------------------------------------------------------- 1 | .. include:: ../../CHANGELOG.rst 2 | -------------------------------------------------------------------------------- /docs/sources/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # py_trees documentation build configuration file, created by 4 | # sphinx-quickstart on Thu Jul 30 16:43:58 2015. 5 | # 6 | # This file is execfile()d with the current directory set to its 7 | # containing dir. 8 | # 9 | # Note that not all possible configuration values are present in this 10 | # autogenerated file. 11 | # 12 | # All configuration values have a default; values that are commented out 13 | # serve to show the default. 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import os 20 | import sys 21 | import unittest.mock 22 | 23 | ############################################################################## 24 | # Setup 25 | ############################################################################## 26 | 27 | # Paths 28 | project_dir = os.path.abspath( 29 | os.path.join(os.path.abspath(__file__), os.pardir, os.pardir, os.pardir) 30 | ) 31 | 32 | # Version 33 | version_file = os.path.join(project_dir, "py_trees_ros", "version.py") 34 | with open(version_file) as f: 35 | exec(f.read()) # makes __version__ available 36 | 37 | # Autodoc 38 | sys.path.insert(0, project_dir) 39 | 40 | ############################################################################## 41 | # Project Information 42 | ############################################################################## 43 | 44 | project = u'py_trees_ros' 45 | copyright = u'2023, Daniel Stonier' 46 | author = "Daniel Stonier" 47 | 48 | version = __version__ 49 | release = version 50 | 51 | ############################################################################## 52 | # Extensions 53 | ############################################################################## 54 | 55 | extensions = [ 56 | 'sphinx.ext.napoleon', 57 | 'sphinx.ext.graphviz', 58 | 'sphinx.ext.intersphinx', 59 | 'sphinx.ext.coverage', 60 | 'sphinx.ext.doctest', 61 | 'sphinx.ext.ifconfig', 62 | 'sphinx.ext.autodoc', 63 | 'sphinx.ext.autosummary', 64 | 'sphinx.ext.mathjax', 65 | 'sphinx.ext.todo', 66 | 'sphinx.ext.viewcode', 67 | 'sphinxarg.ext', 68 | 'sphinx.ext.autodoc.typehints' 69 | # Broken with latest sphinx / mock 70 | # https://github.com/tox-dev/sphinx-autodoc-typehints/issues/220 71 | # 'sphinx_autodoc_typehints', 72 | ] 73 | 74 | ############################################################################## 75 | # Extension Configuration 76 | ############################################################################## 77 | 78 | autodoc_typehints = "description" 79 | 80 | # True to use the :ivar: role for instance variables. 81 | # False to use the .. attribute:: directive instead. 82 | napoleon_use_ivar = True 83 | 84 | # If you don't add this, todos don't appear 85 | todo_include_todos = True 86 | 87 | ############################################################################## 88 | # Regular Sphinx Configuration 89 | ############################################################################## 90 | 91 | templates_path = ["_templates"] 92 | exclude_patterns = ["_build", "weblinks.rst"] 93 | language = "en" 94 | 95 | ################################################################################ 96 | # HTML 97 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 98 | ################################################################################ 99 | 100 | html_theme = "sphinx_rtd_theme" 101 | 102 | # Add any paths that contain custom static files (such as style sheets) here, 103 | # relative to this directory. They are copied after the builtin static files, 104 | # so a file named "default.css" will overwrite the builtin "default.css". 105 | html_static_path = [] 106 | 107 | # If true, “(C) Copyright …” is shown in the HTML footer. Default is True. 108 | html_show_copyright = False 109 | 110 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 111 | html_show_sphinx = False 112 | 113 | ############################################################################## 114 | # Intersphinx 115 | ############################################################################## 116 | 117 | intersphinx_mapping = { 118 | 'python': ('https://docs.python.org/3', None), 119 | 'py_trees': ('https://py-trees.readthedocs.io/en/release-2.2.x', None), 120 | 'py_trees_ros_tutorials': ('https://py-trees-ros-tutorials.readthedocs.io/en/release-2.1.x', None), 121 | 'rclpy': ('https://docs.ros2.org/foxy/api/rclpy/', None), 122 | } 123 | 124 | 125 | ############################################################################## 126 | # Autodoc Mocks 127 | ############################################################################## 128 | 129 | 130 | # Caveats: Currently autodoc is failing on classes which 131 | # inherit from a mocked class. I don't know if this is a bug 132 | # in sphinx, or something I'm not implementing correctly. 133 | 134 | # Workaround: I only inherit from py_trees, so make sure that 135 | # is in the environment and not mocked. 136 | 137 | 138 | MOCK_MODULES = [ 139 | 'action_msgs', 'action_msgs.msg', 140 | 'diagnostic_msgs', 'diagnostic_msgs.msg', 141 | 'geometry_msgs', 'geometry_msgs.msg', 142 | 'py_trees_ros_interfaces', 'py_trees_ros_interfaces.action', 143 | 'py_trees_ros_interfaces.msg', 'py_trees_ros_interfaces.srv', 144 | 'rcl_interfaces', 'rcl_interfaces.msg', 145 | 'ros2cli', 'ros2cli.node', 'ros2cli.node.strategy', 146 | 'ros2topic', 'ros2topic.api', 147 | 'sensor_msgs', 'sensor_msgs.msg', 148 | 'std_msgs', 'std_msgs.msg', 149 | 'std_srvs', 'std_srvs.srv', 150 | 'tf2_ros', 151 | 'unique_identifier_msgs', 'unique_identifier_msgs.msg' 152 | ] 153 | 154 | for mod_name in MOCK_MODULES: 155 | sys.modules[mod_name] = unittest.mock.Mock() 156 | 157 | # A few modules are not available for import in the local environment 158 | # but do have information available via intersphinx mappings (and hence 159 | # don't need to be mocked as above). These will break autodoc when it 160 | # tries to import them, so at least mock the imports. 161 | autodoc_mock_imports = [ 162 | "rclpy" 163 | ] 164 | -------------------------------------------------------------------------------- /docs/sources/dot/demo-exchange.dot: -------------------------------------------------------------------------------- 1 | digraph sequence { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Sequence [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 6 | Carbonara [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 7 | Sequence -> Carbonara; 8 | Gnocchi [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Sequence -> Gnocchi; 10 | } 11 | -------------------------------------------------------------------------------- /docs/sources/dot/demo_tree.dot: -------------------------------------------------------------------------------- 1 | digraph root { 2 | Root [shape=house, fontsize=11, style=filled, fillcolor=cyan]; 3 | EveryN [shape=ellipse, fontsize=11, style=filled, fillcolor=gray]; 4 | Root -> EveryN; 5 | Sequence [shape=box, fontsize=11, style=filled, fillcolor=orange]; 6 | Root -> Sequence; 7 | Guard [shape=ellipse, fontsize=11, style=filled, fillcolor=gray]; 8 | Sequence -> Guard; 9 | Periodic [shape=ellipse, fontsize=11, style=filled, fillcolor=gray]; 10 | Sequence -> Periodic; 11 | Finisher [shape=ellipse, fontsize=11, style=filled, fillcolor=gray]; 12 | Sequence -> Finisher; 13 | Idle [shape=ellipse, fontsize=11, style=filled, fillcolor=gray]; 14 | Root -> Idle; 15 | } 16 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-eight-scan.dot: -------------------------------------------------------------------------------- 1 | digraph scan_or_die { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | "Scan or Die" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 6 | "Ere we Go" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | "Scan or Die" -> "Ere we Go"; 8 | UnDock [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | "Ere we Go" -> UnDock; 10 | "Scan or Be Cancelled" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 11 | "Ere we Go" -> "Scan or Be Cancelled"; 12 | "Cancelling?" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 13 | "Scan or Be Cancelled" -> "Cancelling?"; 14 | "Cancel?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 15 | "Cancelling?" -> "Cancel?"; 16 | "Move Home" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | "Cancelling?" -> "Move Home"; 18 | "Move Out and Scan" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 19 | "Scan or Be Cancelled" -> "Move Out and Scan"; 20 | "Move Out" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 21 | "Move Out and Scan" -> "Move Out"; 22 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 23 | "Move Out and Scan" -> Scanning; 24 | "Context Switch" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 25 | Scanning -> "Context Switch"; 26 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 27 | Scanning -> Rotate; 28 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 29 | Scanning -> "Flash Blue"; 30 | "Move Home*" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 31 | "Move Out and Scan" -> "Move Home*"; 32 | Dock [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 33 | "Ere we Go" -> Dock; 34 | Celebrate [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 35 | "Ere we Go" -> Celebrate; 36 | "Flash Green" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 37 | Celebrate -> "Flash Green"; 38 | Pause [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 39 | Celebrate -> Pause; 40 | "Uh Oh" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 41 | "Scan or Die" -> "Uh Oh"; 42 | } 43 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-eight.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Scan2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Scan2BB; 10 | Cancel2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Topics2BB -> Cancel2BB; 12 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 13 | Topics2BB -> Battery2BB; 14 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 15 | Tutorial -> Priorities; 16 | "Battery Emergency" [fillcolor=ghostwhite, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | Priorities -> "Battery Emergency"; 18 | "Battery Ok?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 19 | "Battery Emergency" -> "Battery Ok?"; 20 | "Flash Red" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 21 | "Battery Emergency" -> "Flash Red"; 22 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 23 | Priorities -> Idle; 24 | } 25 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-five-guard.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | "Scan (Guard)" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 6 | "Scan?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 7 | "Scan (Guard)" -> "Scan?"; 8 | "Preempt?" [fillcolor=gray20, fontcolor=dodgerblue, fontsize=11, shape=box, style=filled]; 9 | "Scan (Guard)" -> "Preempt?"; 10 | } 11 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-five-preempt.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | "Preempt?" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 6 | "Scan?\n(Success is Running)" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 7 | "Preempt?" -> "Scan?\n(Success is Running)"; 8 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 9 | "Preempt?" -> Scanning; 10 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Scanning -> Rotate; 12 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 13 | Scanning -> "Flash Blue"; 14 | } 15 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-five.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Scan2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Scan2BB; 10 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Topics2BB -> Battery2BB; 12 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 13 | Tutorial -> Priorities; 14 | "Battery Emergency" [fillcolor=ghostwhite, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 15 | Priorities -> "Battery Emergency"; 16 | "Battery Ok?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | "Battery Emergency" -> "Battery Ok?"; 18 | "Flash Red" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 19 | "Battery Emergency" -> "Flash Red"; 20 | Scan [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 21 | Priorities -> Scan; 22 | "Scan?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 23 | Scan -> "Scan?"; 24 | "Preempt?" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 25 | Scan -> "Preempt?"; 26 | "Scan?*" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 27 | "Preempt?" -> "Scan?*"; 28 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 29 | "Preempt?" -> Scanning; 30 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 31 | Scanning -> Rotate; 32 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 33 | Scanning -> "Flash Blue"; 34 | Celebrate [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 35 | Scan -> Celebrate; 36 | "Flash Green" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 37 | Celebrate -> "Flash Green"; 38 | Pause [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 39 | Celebrate -> Pause; 40 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 41 | Priorities -> Idle; 42 | } 43 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-one.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Battery2BB; 10 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 11 | Tutorial -> Priorities; 12 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 13 | Priorities -> Idle; 14 | } 15 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-seven.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Scan2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Scan2BB; 10 | Cancel2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Topics2BB -> Cancel2BB; 12 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 13 | Topics2BB -> Battery2BB; 14 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 15 | Tutorial -> Priorities; 16 | "Battery Emergency" [fillcolor=ghostwhite, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | Priorities -> "Battery Emergency"; 18 | "Battery Ok?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 19 | "Battery Emergency" -> "Battery Ok?"; 20 | "Flash Red" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 21 | "Battery Emergency" -> "Flash Red"; 22 | Scan [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 23 | Priorities -> Scan; 24 | "Scan?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 25 | Scan -> "Scan?"; 26 | "Scan or Die" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 27 | Scan -> "Scan or Die"; 28 | "Ere we Go" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 29 | "Scan or Die" -> "Ere we Go"; 30 | UnDock [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 31 | "Ere we Go" -> UnDock; 32 | "Scan or Be Cancelled" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 33 | "Ere we Go" -> "Scan or Be Cancelled"; 34 | "Cancelling?" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 35 | "Scan or Be Cancelled" -> "Cancelling?"; 36 | "Cancel?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 37 | "Cancelling?" -> "Cancel?"; 38 | "Move Home" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 39 | "Cancelling?" -> "Move Home"; 40 | "Move Out and Scan" [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 41 | "Scan or Be Cancelled" -> "Move Out and Scan"; 42 | "Move Out" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 43 | "Move Out and Scan" -> "Move Out"; 44 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 45 | "Move Out and Scan" -> Scanning; 46 | "Context Switch" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 47 | Scanning -> "Context Switch"; 48 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 49 | Scanning -> Rotate; 50 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 51 | Scanning -> "Flash Blue"; 52 | "Move Home*" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 53 | "Move Out and Scan" -> "Move Home*"; 54 | Dock [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 55 | "Ere we Go" -> Dock; 56 | Celebrate [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 57 | "Ere we Go" -> Celebrate; 58 | "Flash Green" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 59 | Celebrate -> "Flash Green"; 60 | Pause [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 61 | Celebrate -> Pause; 62 | "Uh Oh" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 63 | "Scan or Die" -> "Uh Oh"; 64 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 65 | Priorities -> Idle; 66 | } 67 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-six-context-switch.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | "Context Switch" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 7 | Scanning -> "Context Switch"; 8 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Scanning -> Rotate; 10 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Scanning -> "Flash Blue"; 12 | } 13 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-six.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Scan2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Scan2BB; 10 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 11 | Topics2BB -> Battery2BB; 12 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 13 | Tutorial -> Priorities; 14 | "Battery Emergency" [fillcolor=ghostwhite, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 15 | Priorities -> "Battery Emergency"; 16 | "Battery Ok?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | "Battery Emergency" -> "Battery Ok?"; 18 | "Flash Red" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 19 | "Battery Emergency" -> "Flash Red"; 20 | Scan [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 21 | Priorities -> Scan; 22 | "Scan?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 23 | Scan -> "Scan?"; 24 | "Preempt?" [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 25 | Scan -> "Preempt?"; 26 | "Scan?*" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 27 | "Preempt?" -> "Scan?*"; 28 | Scanning [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 29 | "Preempt?" -> Scanning; 30 | "Context Switch" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 31 | Scanning -> "Context Switch"; 32 | Rotate [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 33 | Scanning -> Rotate; 34 | "Flash Blue" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 35 | Scanning -> "Flash Blue"; 36 | Celebrate [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 37 | Scan -> Celebrate; 38 | "Flash Green" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 39 | Celebrate -> "Flash Green"; 40 | Pause [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 41 | Celebrate -> Pause; 42 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 43 | Priorities -> Idle; 44 | } 45 | -------------------------------------------------------------------------------- /docs/sources/dot/tutorial-two.dot: -------------------------------------------------------------------------------- 1 | digraph tutorial { 2 | graph [fontname="times-roman"]; 3 | node [fontname="times-roman"]; 4 | edge [fontname="times-roman"]; 5 | Tutorial [fillcolor=gold, fontcolor=black, fontsize=11, shape=note, style=filled]; 6 | Topics2BB [fillcolor=orange, fontcolor=black, fontsize=11, shape=box, style=filled]; 7 | Tutorial -> Topics2BB; 8 | Battery2BB [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 9 | Topics2BB -> Battery2BB; 10 | Priorities [fillcolor=cyan, fontcolor=black, fontsize=11, shape=octagon, style=filled]; 11 | Tutorial -> Priorities; 12 | "Battery Emergency" [fillcolor=ghostwhite, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 13 | Priorities -> "Battery Emergency"; 14 | "Battery Ok?" [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 15 | "Battery Emergency" -> "Battery Ok?"; 16 | FlashLEDs [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 17 | "Battery Emergency" -> FlashLEDs; 18 | Idle [fillcolor=gray, fontcolor=black, fontsize=11, shape=ellipse, style=filled]; 19 | Priorities -> Idle; 20 | } 21 | -------------------------------------------------------------------------------- /docs/sources/features.rst: -------------------------------------------------------------------------------- 1 | Features 2 | ======== 3 | 4 | Behaviours 5 | ---------- 6 | 7 | .. autosummary:: 8 | 9 | py_trees_ros.action_clients.FromBlackboard 10 | py_trees_ros.action_clients.FromConstant 11 | py_trees_ros.battery.ToBlackboard 12 | py_trees_ros.publishers.FromBlackboard 13 | py_trees_ros.subscribers.CheckData 14 | py_trees_ros.subscribers.EventToBlackboard 15 | py_trees_ros.subscribers.ToBlackboard 16 | py_trees_ros.subscribers.WaitForData 17 | py_trees_ros.transforms.FromBlackboard 18 | py_trees_ros.transforms.ToBlackboard 19 | 20 | Blackboards 21 | ----------- 22 | 23 | .. automodule:: py_trees_ros.blackboard 24 | :noindex: 25 | 26 | Trees 27 | ----- 28 | 29 | .. automodule:: py_trees_ros.trees 30 | :noindex: 31 | 32 | Programs 33 | -------- 34 | 35 | .. _py-trees-blackboard-watcher: 36 | 37 | py-trees-blackboard-watcher 38 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 39 | 40 | .. automodule:: py_trees_ros.programs.blackboard_watcher 41 | :synopsis: call Exchange services to open up a watcher to a blackboard 42 | :noindex: 43 | 44 | .. _py-trees-tree-watcher: 45 | 46 | py-trees-tree-watcher 47 | ^^^^^^^^^^^^^^^^^^^^^ 48 | 49 | .. automodule:: py_trees_ros.programs.tree_watcher 50 | :synopsis: visualise the tree as dot graph, ascii tree, or ascii snapshot 51 | :noindex: 52 | 53 | -------------------------------------------------------------------------------- /docs/sources/images/blackboard-watcher.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/sources/images/blackboard-watcher.gif -------------------------------------------------------------------------------- /docs/sources/images/tree-watcher.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/sources/images/tree-watcher.gif -------------------------------------------------------------------------------- /docs/sources/images/trees.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/sources/images/trees.png -------------------------------------------------------------------------------- /docs/sources/images/trees_dot_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/docs/sources/images/trees_dot_graph.png -------------------------------------------------------------------------------- /docs/sources/index.rst: -------------------------------------------------------------------------------- 1 | .. py_trees documentation master file, created by 2 | sphinx-quickstart on Thu Jul 30 16:43:58 2015. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | .. _index-section: 7 | 8 | Py Trees for ROS 9 | ================ 10 | 11 | Behaviour trees for ros in python. 12 | 13 | .. toctree:: 14 | :maxdepth: 2 15 | :caption: Contents 16 | 17 | about 18 | features 19 | modules 20 | changelog 21 | 22 | Indices and tables 23 | ================== 24 | 25 | * :ref:`genindex` 26 | * :ref:`modindex` 27 | * :ref:`search` 28 | 29 | -------------------------------------------------------------------------------- /docs/sources/modules.rst: -------------------------------------------------------------------------------- 1 | .. _modules-section-label: 2 | 3 | Module API 4 | ========== 5 | 6 | py_trees_ros 7 | ------------ 8 | 9 | .. automodule:: py_trees_ros 10 | :synopsis: is your robot misbehaving? 11 | 12 | py_trees_ros.action_clients 13 | --------------------------- 14 | 15 | .. automodule:: py_trees_ros.action_clients 16 | :members: 17 | :show-inheritance: 18 | :synopsis: behaviours that work with action clients 19 | 20 | py_trees_ros.battery 21 | -------------------- 22 | 23 | .. automodule:: py_trees_ros.battery 24 | :members: 25 | :show-inheritance: 26 | :synopsis: helpers for getting the most out of your battery 27 | 28 | py_trees_ros.blackboard 29 | ----------------------- 30 | 31 | .. automodule:: py_trees_ros.blackboard 32 | :members: 33 | :show-inheritance: 34 | :synopsis: the ros blackboard and the blackboard watcher 35 | 36 | py_trees_ros.conversions 37 | ------------------------ 38 | 39 | .. automodule:: py_trees_ros.conversions 40 | :members: 41 | :show-inheritance: 42 | :synopsis: py_trees - ros messages conversion library 43 | 44 | py_trees_ros.exceptions 45 | ----------------------- 46 | 47 | .. automodule:: py_trees_ros.exceptions 48 | :members: 49 | :show-inheritance: 50 | :synopsis: custom exceptions for py_trees_ros 51 | 52 | py_trees_ros.publishers 53 | ----------------------- 54 | 55 | .. automodule:: py_trees_ros.publishers 56 | :members: 57 | :show-inheritance: 58 | :synopsis: publish data to the ROS network 59 | 60 | py_trees_ros.subscribers 61 | ------------------------ 62 | 63 | .. automodule:: py_trees_ros.subscribers 64 | :members: 65 | :show-inheritance: 66 | :synopsis: put a lid on the asynchronicity 67 | 68 | py_trees_ros.transforms 69 | ----------------------- 70 | 71 | .. automodule:: py_trees_ros.transforms 72 | :members: 73 | :show-inheritance: 74 | :synopsis: lookup and send transforms to and from the blackboard 75 | 76 | 77 | py_trees_ros.trees 78 | ------------------ 79 | 80 | .. automodule:: py_trees_ros.trees 81 | :members: 82 | :show-inheritance: 83 | :synopsis: extensions with ros handles for debugging, visualisation messages and more 84 | 85 | py_trees_ros.utilities 86 | ---------------------- 87 | 88 | .. automodule:: py_trees_ros.utilities 89 | :members: 90 | :show-inheritance: 91 | :synopsis: assorted utility functions 92 | 93 | py_trees_ros.visitors 94 | --------------------- 95 | 96 | .. automodule:: py_trees_ros.visitors 97 | :members: 98 | :show-inheritance: 99 | :synopsis: entities that visit behaviours as a tree is traversed 100 | 101 | py_trees_ros.programs 102 | --------------------- 103 | 104 | .. automodule:: py_trees_ros.programs 105 | :synopsis: command line utilities for py_trees_ros 106 | 107 | py_trees_ros.programs.blackboard_watcher 108 | ---------------------------------------- 109 | 110 | .. automodule:: py_trees_ros.programs.blackboard_watcher 111 | :members: 112 | :special-members: 113 | :show-inheritance: 114 | :synopsis: command line blackboard watcher 115 | 116 | py_trees_ros.programs.tree_watcher 117 | ---------------------------------- 118 | 119 | .. automodule:: py_trees_ros.programs.tree_watcher 120 | :members: 121 | :special-members: 122 | :show-inheritance: 123 | :synopsis: command line tree watcher 124 | -------------------------------------------------------------------------------- /docs/sources/weblinks.rst: -------------------------------------------------------------------------------- 1 | .. 2 | This file contains a common collection of web links. Include it 3 | wherever these links are needed. 4 | It is explicitly excluded in ``conf.py``, because it does not 5 | appear anywhere in the TOC tree. 6 | 7 | .. _`operator module`: https://docs.python.org/3/library/operator.html 8 | -------------------------------------------------------------------------------- /docs/venv.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Script for setting up the development environment. 4 | #source /usr/share/virtualenvwrapper/virtualenvwrapper.sh 5 | 6 | PROJECT=py_trees_ros 7 | SRC_DIR=$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd) 8 | VENV_DIR=${SRC_DIR}/.venv 9 | 10 | ############################################################################## 11 | # Colours 12 | ############################################################################## 13 | 14 | BOLD="\e[1m" 15 | 16 | CYAN="\e[36m" 17 | GREEN="\e[32m" 18 | RED="\e[31m" 19 | YELLOW="\e[33m" 20 | 21 | RESET="\e[0m" 22 | 23 | padded_message () 24 | { 25 | line="........................................" 26 | printf "%s %s${2}\n" ${1} "${line:${#1}}" 27 | } 28 | 29 | pretty_header () 30 | { 31 | echo -e "${BOLD}${1}${RESET}" 32 | } 33 | 34 | pretty_print () 35 | { 36 | echo -e "${GREEN}${1}${RESET}" 37 | } 38 | 39 | pretty_warning () 40 | { 41 | echo -e "${YELLOW}${1}${RESET}" 42 | } 43 | 44 | pretty_error () 45 | { 46 | echo -e "${RED}${1}${RESET}" 47 | } 48 | 49 | ############################################################################## 50 | # Methods 51 | ############################################################################## 52 | 53 | install_package () 54 | { 55 | PACKAGE_NAME=$1 56 | dpkg -s ${PACKAGE_NAME} > /dev/null 57 | if [ $? -ne 0 ]; then 58 | sudo apt-get -q -y install ${PACKAGE_NAME} > /dev/null 59 | else 60 | pretty_print " $(padded_message ${PACKAGE_NAME} "found")" 61 | return 0 62 | fi 63 | if [ $? -ne 0 ]; then 64 | pretty_error " $(padded_message ${PACKAGE_NAME} "failed")" 65 | return 1 66 | fi 67 | pretty_warning " $(padded_message ${PACKAGE_NAME} "installed")" 68 | return 0 69 | } 70 | 71 | ############################################################################## 72 | 73 | ############################# 74 | # Checks 75 | ############################# 76 | 77 | [[ "${BASH_SOURCE[0]}" != "${0}" ]] && SOURCED=1 78 | if [ -z "$SOURCED" ]; then 79 | pretty_error "This script needs to be sourced, i.e. source './setup.bash', not './setup.bash'" 80 | exit 1 81 | fi 82 | 83 | ############################# 84 | # System Dependencies 85 | ############################# 86 | 87 | pretty_header "System Dependencies" 88 | install_package python3-venv || return 89 | 90 | ############################# 91 | # Virtual Env 92 | ############################# 93 | 94 | pretty_header "Virtual Environment" 95 | 96 | if [ -x ${VENV_DIR}/bin/pip3 ]; then 97 | pretty_print " $(padded_message "virtual_environment" "found [${VENV_DIR}]")" 98 | else 99 | python3 -m venv ${VENV_DIR} 100 | pretty_warning " $(padded_message "virtual_environment" "created [${VENV_DIR}]")" 101 | fi 102 | 103 | source ${VENV_DIR}/bin/activate 104 | 105 | ############################# 106 | # Pypi Dependencies 107 | ############################# 108 | 109 | pretty_header "PyPi Dependencies" 110 | 111 | # upgrade pip3 112 | python3 -m pip install -U pip 113 | 114 | # build environment depedencies 115 | pip3 install wheel 116 | pip3 install "setuptools==45.2" 117 | 118 | # Get all dependencies for testing, doc generation 119 | 120 | # Install doc dependencies 121 | pip3 install -r requirements.txt 122 | 123 | # Install the project 124 | python3 ../setup.py develop 125 | 126 | echo "" 127 | echo "Leave the virtual environment with 'deactivate'" 128 | echo "" 129 | echo "I'm grooty, you should be too." 130 | echo "" 131 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | py_trees_ros 5 | 2.3.0 6 | 7 | ROS2 extensions and behaviours for py_trees. 8 | 9 | 10 | Daniel Stonier 11 | Michal Staniaszek 12 | Naveed Usmani 13 | 14 | Daniel Stonier 15 | Sebastian Castro 16 | 17 | BSD 18 | 19 | https://py-trees-ros.readthedocs.io/en/release-1.2.x/ 20 | https://github.com/stonier/py_trees_ros 21 | https://github.com/stonier/py_trees_ros/issues 22 | 23 | python3-setuptools 24 | 25 | python3-pytest 26 | 27 | python3-sphinx 28 | python3-sphinx-argparse 29 | python3-sphinx-rtd-theme 30 | 31 | 32 | py_trees 33 | py_trees_ros_interfaces 34 | rcl_interfaces 35 | rclpy 36 | ros2topic 37 | std_msgs 38 | std_srvs 39 | unique_identifier_msgs 40 | 41 | 42 | geometry_msgs 43 | sensor_msgs 44 | tf2_ros_py 45 | 46 | 47 | 48 | 49 | ament_python 50 | 51 | 52 | -------------------------------------------------------------------------------- /py_trees_ros/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # License: BSD 3 | # https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE 4 | # 5 | ############################################################################## 6 | # Documentation 7 | ############################################################################## 8 | 9 | """ 10 | ROS extensions, behaviours and utilities for py_trees. 11 | """ 12 | 13 | ############################################################################## 14 | # Imports 15 | ############################################################################## 16 | 17 | from . import action_clients 18 | from . import actions # to be deprecated in 2.1.x or later 19 | from . import battery 20 | from . import blackboard 21 | from . import conversions 22 | from . import exceptions 23 | from . import mock 24 | from . import programs 25 | from . import publishers 26 | from . import service_clients 27 | from . import subscribers 28 | from . import transforms 29 | from . import trees 30 | from . import utilities 31 | from . import visitors 32 | 33 | ############################################################################## 34 | # Version 35 | ############################################################################## 36 | 37 | from .version import __version__ 38 | -------------------------------------------------------------------------------- /py_trees_ros/actions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Behaviours for ROS actions. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | from . import action_clients 20 | 21 | ############################################################################## 22 | # Behaviours 23 | ############################################################################## 24 | 25 | 26 | # to be deprecated in 2.1.x or later 27 | ActionClient = action_clients.FromConstant 28 | -------------------------------------------------------------------------------- /py_trees_ros/battery.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Getting the most out of your battery. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import py_trees 20 | import rclpy.qos 21 | import sensor_msgs.msg as sensor_msgs 22 | 23 | from . import subscribers 24 | 25 | ############################################################################## 26 | # Behaviours 27 | ############################################################################## 28 | 29 | 30 | class ToBlackboard(subscribers.ToBlackboard): 31 | """ 32 | Subscribes to the battery message and writes battery data to the blackboard. 33 | Also adds a warning flag to the blackboard if the battery 34 | is low - note that it does some buffering against ping-pong problems so the warning 35 | doesn't trigger on/off rapidly when close to the threshold. 36 | 37 | When ticking, updates with :attr:`~py_trees.common.Status.RUNNING` if it got no data, 38 | :attr:`~py_trees.common.Status.SUCCESS` otherwise. 39 | 40 | Blackboard Variables: 41 | * battery (:class:`sensor_msgs.msg.BatteryState`)[w]: the raw battery message 42 | * battery_low_warning (:obj:`bool`)[w]: False if battery is ok, True if critically low 43 | 44 | Args: 45 | name: name of the behaviour 46 | topic_name: name of the battery state topic 47 | qos_profile: qos profile for the subscriber 48 | threshold: percentage level threshold for flagging as low (0-100) 49 | """ 50 | def __init__(self, 51 | name: str, 52 | topic_name: str, 53 | qos_profile: rclpy.qos.QoSProfile, 54 | threshold: float=30.0): 55 | super().__init__(name=name, 56 | topic_name=topic_name, 57 | topic_type=sensor_msgs.BatteryState, 58 | qos_profile=qos_profile, 59 | blackboard_variables={"battery": None}, 60 | clearing_policy=py_trees.common.ClearingPolicy.NEVER 61 | ) 62 | self.blackboard.register_key( 63 | key="battery_low_warning", 64 | access=py_trees.common.Access.WRITE 65 | ) 66 | self.blackboard.battery = sensor_msgs.BatteryState() 67 | self.blackboard.battery.percentage = 0.0 68 | self.blackboard.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_UNKNOWN # noqa 69 | self.blackboard.battery_low_warning = False # decision making 70 | self.threshold = threshold 71 | 72 | def update(self) -> py_trees.common.Status: 73 | """ 74 | Call the parent to write the raw data to the blackboard and then check against the 75 | threshold to determine if the low warning flag should also be updated. 76 | 77 | Returns: 78 | :attr:`~py_trees.common.Status.SUCCESS` if a message was written, :attr:`~py_trees.common.Status.RUNNING` otherwise. 79 | """ 80 | self.logger.debug("%s.update()" % self.__class__.__name__) 81 | status = super(ToBlackboard, self).update() 82 | if status != py_trees.common.Status.RUNNING: 83 | # we got something 84 | if self.blackboard.battery.percentage > self.threshold + 5.0: 85 | self.blackboard.battery_low_warning = False 86 | elif self.blackboard.battery.percentage < self.threshold: 87 | self.blackboard.battery_low_warning = True 88 | # TODO: make this throttled 89 | self.node.get_logger().error("{}: battery level is low!".format(self.name)) 90 | # else don't do anything in between - i.e. avoid the ping pong problems 91 | 92 | self.feedback_message = "Battery level is low" if self.blackboard.battery_low_warning else "Battery level is ok" 93 | return status 94 | -------------------------------------------------------------------------------- /py_trees_ros/exceptions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Custom exception types for py_trees_ros. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | 20 | class MultipleFoundError(Exception): 21 | """ 22 | Middleware connection found when but one was expected. 23 | """ 24 | pass 25 | 26 | 27 | class NotFoundError(Exception): 28 | """ 29 | Middleware connection not found. 30 | """ 31 | pass 32 | 33 | 34 | class NotReadyError(Exception): 35 | """ 36 | Typically used when methods have been called that expect, but have not 37 | pre-engaged in the ROS2 specific setup typical of py_trees_ros classes 38 | and behaviours. 39 | """ 40 | pass 41 | 42 | 43 | class ServiceError(Exception): 44 | """ 45 | Failure in a service request-response process (usually no response). 46 | """ 47 | pass 48 | 49 | 50 | class TimedOutError(Exception): 51 | """ 52 | Timed out waiting (typically) for middleware connections to be established. 53 | """ 54 | pass 55 | -------------------------------------------------------------------------------- /py_trees_ros/mock/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # License: BSD 3 | # https://raw.githubusercontent.com/stonier/py_trees_ros/devel/LICENSE 4 | # 5 | ############################################################################## 6 | # Documentation 7 | ############################################################################## 8 | 9 | """ 10 | This package contains mock ROS nodes for py_trees tests. 11 | """ 12 | ############################################################################## 13 | # Imports 14 | ############################################################################## 15 | 16 | from . import actions 17 | from . import dock 18 | from . import services 19 | from . import set_bool 20 | -------------------------------------------------------------------------------- /py_trees_ros/mock/actions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros_tutorials/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Action server/client templates. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import rclpy 20 | import rclpy.action 21 | import rclpy.callback_groups 22 | import rclpy.parameter 23 | import threading 24 | import time 25 | 26 | ############################################################################## 27 | # Action Server 28 | ############################################################################## 29 | # 30 | # References: 31 | # 32 | # action client : https://github.com/ros2/rclpy/blob/master/rclpy/rclpy/action/client.py 33 | # action server : https://github.com/ros2/rclpy/blob/master/rclpy/rclpy/action/server.py 34 | # action client PR : https://github.com/ros2/rclpy/pull/257 35 | # action server PR : https://github.com/ros2/rclpy/pull/270 36 | # action examples : https://github.com/ros2/examples/tree/master/rclpy/actions 37 | # 38 | # Note: 39 | # currently: py_trees_ros_interfaces.action.Dock_Goal.Request() 40 | # to come(?): py_trees_ros_interfaces.action.Dock.GoalRequestService.Request() 41 | 42 | 43 | class GenericServer(object): 44 | """ 45 | Generic action server that can be subclassed to quickly create action 46 | servers of varying types in a mock simulation. 47 | 48 | Dynamic Reconfigure: 49 | * **~duration** (:obj:`float`) 50 | 51 | * reconfigure the duration to be used for the next goal execution 52 | 53 | Args: 54 | node_name (:obj:`str`): name to use for the node (e.g. docking_controller) 55 | action_name (:obj:`str`): name of the action server (e.g. dock) 56 | action_type (:obj:`any`): type of the action server (e.g. py_trees_ros_interfaces.Dock 57 | custom_execute_callback (:obj:`func`): callback to be executed inside the execute loop, no args 58 | generate_cancelled_result (action_type.Result): custom result method 59 | generate_preempted_result (action_type.Result): custom result method 60 | generate_success_result (action_type.Result): custom result method 61 | goal_recieved_callback(:obj:`func`): callback to be executed immediately upon receiving a goal 62 | duration (:obj:`float`): forcibly override the dyn reconf time for a goal to complete (seconds) 63 | 64 | Use the ``dashboard`` to dynamically reconfigure the parameters. 65 | 66 | There are some shortcomings in this class that could be addressed to make it more robust: 67 | 68 | - check for matching goal id's when disrupting execution 69 | - execute multiple requests in parallel / pre-emption (not even tried) 70 | """ 71 | def __init__(self, 72 | node_name, 73 | action_name, 74 | action_type, # e.g. py_trees_ros_interfaces.action.Dock 75 | generate_feedback_message=None, 76 | generate_cancelled_result=None, 77 | generate_preempted_result=None, 78 | generate_success_result=None, 79 | goal_received_callback=lambda request: None, 80 | duration=None): 81 | self.node = rclpy.create_node( 82 | node_name, 83 | parameter_overrides=[ 84 | rclpy.parameter.Parameter( 85 | 'duration', 86 | rclpy.parameter.Parameter.Type.DOUBLE, 87 | 5.0 # seconds 88 | ), 89 | ], 90 | automatically_declare_parameters_from_overrides=True 91 | ) 92 | # override 93 | if duration is not None: 94 | self.node.set_parameters([ 95 | rclpy.parameter.Parameter( 96 | 'duration', 97 | rclpy.parameter.Parameter.Type.DOUBLE, 98 | duration # seconds 99 | ), 100 | ]) 101 | # Needs a member variable to capture the dynamic parameter 102 | # value when accepting the goal and pass that to the execution 103 | # of the goal. Not necessary to initialise here, but done 104 | # for completeness 105 | self.duration = self.node.get_parameter("duration").value 106 | 107 | self.goal_handle = None 108 | self.goal_lock = threading.Lock() 109 | self.frequency = 3.0 # hz 110 | self.percent_completed = 0 111 | 112 | self.action_type = action_type 113 | if generate_feedback_message is None: 114 | self.generate_feedback_message = lambda: self.action_type.Feedback() 115 | else: 116 | self.generate_feedback_message = generate_feedback_message 117 | if generate_cancelled_result is None: 118 | self.generate_cancelled_result = lambda: self.action_type.Result(message="goal cancelled") 119 | else: 120 | self.generate_cancelled_result = generate_cancelled_result 121 | if generate_preempted_result is None: 122 | self.generate_preempted_result = lambda: self.action_type.Result(message="goal pre-empted") 123 | else: 124 | self.generate_preempted_result = generate_preempted_result 125 | if generate_success_result is None: 126 | self.generate_success_result = lambda: self.action_type.Result(message="goal executed with success") 127 | else: 128 | self.generate_success_result = generate_success_result 129 | self.goal_received_callback = goal_received_callback 130 | self.goal_handle = None 131 | 132 | self.action_server = rclpy.action.ActionServer( 133 | node=self.node, 134 | action_type=self.action_type, 135 | action_name=action_name, 136 | callback_group=rclpy.callback_groups.ReentrantCallbackGroup(), # needed? 137 | cancel_callback=self.cancel_callback, 138 | execute_callback=self.execute_goal_callback, 139 | goal_callback=self.goal_callback, 140 | handle_accepted_callback=self.handle_accepted_callback, 141 | result_timeout=10 142 | ) 143 | 144 | def goal_callback(self, goal_request): 145 | """ 146 | Args: 147 | goal_request: of .GoalRequest with members 148 | goal_id (unique_identifier.msgs.UUID) and those specified in the action 149 | """ 150 | self.node.get_logger().info("received a goal") 151 | self.goal_received_callback(goal_request) 152 | self.percent_completed = 0 153 | self.duration = self.node.get_parameter("duration").value 154 | return rclpy.action.server.GoalResponse.ACCEPT 155 | 156 | def cancel_callback( 157 | self, 158 | goal_handle: rclpy.action.server.ServerGoalHandle 159 | ) -> rclpy.action.CancelResponse: 160 | """ 161 | Cancel any currently executing goal 162 | 163 | Args: 164 | cancel_request (:class:`~rclpy.action.server.ServerGoalHandle`): 165 | handle with information about the 166 | goal that is requested to be cancelled 167 | """ 168 | self.node.get_logger().info("cancel requested: [{goal_id}]".format( 169 | goal_id=goal_handle.goal_id)) 170 | return rclpy.action.CancelResponse.ACCEPT 171 | 172 | def execute_goal_callback( 173 | self, 174 | goal_handle: rclpy.action.server.ServerGoalHandle 175 | ): 176 | """ 177 | Check for pre-emption, but otherwise just spin around gradually incrementing 178 | a hypothetical 'percent' done. 179 | 180 | Args: 181 | goal_handle (:class:`~rclpy.action.server.ServerGoalHandle`): the goal handle of the executing action 182 | """ 183 | # goal.details (e.g. pose) = don't care 184 | self.node.get_logger().info("executing a goal") 185 | increment = 100 / (self.frequency * self.duration) 186 | while True: 187 | # TODO: use a rate when they have it 188 | time.sleep(1.0 / self.frequency) 189 | self.percent_completed += increment 190 | with self.goal_lock: 191 | if goal_handle.is_active: 192 | if goal_handle.is_cancel_requested: 193 | result = self.generate_cancelled_result() 194 | message = "goal cancelled at {percentage:.2f}%%".format( 195 | percentage=self.percent_completed) 196 | self.node.get_logger().info(message) 197 | goal_handle.canceled() 198 | return result 199 | elif goal_handle.goal_id != self.goal_handle.goal_id: 200 | result = self.generate_preempted_result() 201 | message = "goal pre-empted at {percentage:.2f}%%".format( 202 | percentage=self.percent_completed) 203 | self.node.get_logger().info(message) 204 | goal_handle.abort() 205 | return result 206 | elif self.percent_completed >= 100.0: 207 | self.percent_completed = 100.0 208 | self.node.get_logger().info("sending feedback 100%%") 209 | result = self.generate_success_result() 210 | message = "goal executed with success" 211 | self.node.get_logger().info(message) 212 | goal_handle.succeed() 213 | return result 214 | else: 215 | self.node.get_logger().info("sending feedback {percentage:.2f}%%".format( 216 | percentage=self.percent_completed)) 217 | goal_handle.publish_feedback( 218 | self.generate_feedback_message() 219 | ) 220 | else: # ! active 221 | self.node.get_logger().info("goal is no longer active, aborting") 222 | result = self.action_type.Result() 223 | return result 224 | 225 | def handle_accepted_callback(self, goal_handle): 226 | self.node.get_logger().info("handle accepted") 227 | with self.goal_lock: 228 | self.goal_handle = goal_handle 229 | goal_handle.execute() 230 | 231 | def abort(self): 232 | """ 233 | This method is typically only used when the system is shutting down and 234 | there is an executing goal that needs to be abruptly terminated. 235 | """ 236 | with self.goal_lock: 237 | if self.goal_handle and self.goal_handle.is_active: 238 | self.node.get_logger().info("aborting...") 239 | self.goal_handle.abort() 240 | 241 | def shutdown(self): 242 | """ 243 | Cleanup 244 | """ 245 | self.action_server.destroy() 246 | self.node.destroy_node() 247 | -------------------------------------------------------------------------------- /py_trees_ros/mock/dock.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Mocks a docking controller 13 | """ 14 | 15 | 16 | ############################################################################## 17 | # Imports 18 | ############################################################################## 19 | 20 | import py_trees_ros_interfaces.action as py_trees_actions 21 | 22 | from . import actions 23 | 24 | ############################################################################## 25 | # Class 26 | ############################################################################## 27 | 28 | 29 | class Dock(actions.GenericServer): 30 | """ 31 | Simple server that docks if the goal is true, undocks otherwise. 32 | """ 33 | def __init__(self, duration=2.0): 34 | super().__init__( 35 | node_name="docking_controller", 36 | action_name="dock", 37 | action_type=py_trees_actions.Dock, 38 | generate_feedback_message=self.generate_feedback_message, 39 | goal_received_callback=self.goal_received_callback, 40 | duration=duration 41 | ) 42 | 43 | def goal_received_callback(self, goal): 44 | if goal.dock: 45 | self.title = "Dock" 46 | else: 47 | self.title = "UnDock" 48 | 49 | def generate_feedback_message(self): 50 | """ 51 | Create some appropriate feedback. 52 | """ 53 | # TODO: send some feedback message 54 | msg = py_trees_actions.Dock.Feedback( # noqa 55 | percentage_completed=self.percent_completed 56 | ) 57 | return msg 58 | -------------------------------------------------------------------------------- /py_trees_ros/mock/services.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros_tutorials/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Service server templates. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import time 20 | 21 | import rclpy 22 | 23 | ############################################################################## 24 | # Service Server 25 | ############################################################################## 26 | # 27 | # References: 28 | # 29 | # service client : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/client.py 30 | # service server : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/service.py 31 | # service examples : https://github.com/ros2/examples/tree/rolling/rclpy/services 32 | 33 | class GenericServer(object): 34 | """ 35 | Generic service server that can be used for testing. 36 | 37 | Args: 38 | node_name (:obj:`str`): name of the node 39 | service_name (:obj:`str`): name of the service 40 | service_type (:obj:`type`): type of the service 41 | sleep_time (:obj:`float`): time to sleep before returning a response 42 | callback (:obj:`Optional[callable]`): callback to execute when the service is called 43 | """ 44 | def __init__(self, 45 | node_name, 46 | service_name, 47 | service_type, 48 | sleep_time=1.0, 49 | callback=None): 50 | self.node = rclpy.create_node(node_name) 51 | 52 | self.sleep_time = sleep_time 53 | self.callback = callback 54 | 55 | # Create the service 56 | self.server = self.node.create_service( 57 | service_type, 58 | service_name, 59 | self.execute_callback 60 | ) 61 | 62 | def execute_callback(self, request, response): 63 | """ 64 | Execute the callback and populate the response. 65 | """ 66 | if self.callback is not None: 67 | return self.callback(request, response) 68 | 69 | # Do nothing 70 | time.sleep(self.sleep_time) 71 | return response 72 | 73 | def shutdown(self): 74 | """ 75 | Cleanup 76 | """ 77 | self.server.destroy() 78 | self.node.destroy_node() -------------------------------------------------------------------------------- /py_trees_ros/mock/set_bool.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Mocks a SetBool service 13 | """ 14 | 15 | 16 | ############################################################################## 17 | # Imports 18 | ############################################################################## 19 | 20 | import time 21 | 22 | from std_srvs.srv import SetBool 23 | 24 | from . import services 25 | 26 | ############################################################################## 27 | # Class 28 | ############################################################################## 29 | 30 | 31 | class SetBoolServer(services.GenericServer): 32 | """ 33 | Simple server that docks if the goal is true, undocks otherwise. 34 | """ 35 | SUCCESS_MESSAGE = "Succeeded in setting bool to True" 36 | FAILURE_MESSAGE = "Failed to set bool to False" 37 | 38 | def __init__(self, sleep_time=1.0): 39 | super().__init__( 40 | node_name="set_bool_server", 41 | service_name="set_bool", 42 | service_type=SetBool, 43 | sleep_time=sleep_time, 44 | callback=self.callback, 45 | ) 46 | 47 | def callback(self, request, response): 48 | time.sleep(self.sleep_time) 49 | if request.data: 50 | response.success = True 51 | response.message = SetBoolServer.SUCCESS_MESSAGE 52 | else: 53 | response.success = False 54 | response.message = SetBoolServer.FAILURE_MESSAGE 55 | return response 56 | -------------------------------------------------------------------------------- /py_trees_ros/programs/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # License: BSD 3 | # https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE 4 | # 5 | ############################################################################## 6 | # Documentation 7 | ############################################################################## 8 | 9 | """ 10 | Entry points to the programs (tools and utilities). 11 | """ 12 | 13 | ############################################################################## 14 | # Imports 15 | ############################################################################## 16 | 17 | from . import blackboard_watcher 18 | from . import echo 19 | from . import multi_talker 20 | from . import tree_watcher 21 | -------------------------------------------------------------------------------- /py_trees_ros/programs/blackboard_watcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | .. argparse:: 13 | :module: py_trees_ros.programs.blackboard_watcher 14 | :func: command_line_argument_parser 15 | :prog: py-trees-blackboard-watcher 16 | 17 | Example interaction with the services of an :class:`py_trees_ros.blackboard.Exchange`: 18 | 19 | .. image:: images/blackboard-watcher.gif 20 | """ 21 | 22 | ############################################################################## 23 | # Imports 24 | ############################################################################## 25 | 26 | import argparse 27 | import functools 28 | import sys 29 | 30 | import rclpy 31 | import std_msgs.msg as std_msgs 32 | 33 | import py_trees.console as console 34 | import py_trees_ros 35 | 36 | ############################################################################## 37 | # Classes 38 | ############################################################################## 39 | 40 | 41 | def description(formatted_for_sphinx): 42 | short = "Open up a window onto the blackboard!\n" 43 | long = ("\nIntrospect on the entire blackboard or a part thereof and receive a stream of\n" 44 | "updates whenever values change.\n" 45 | ) 46 | examples = { 47 | "--list": "list all keys on the blackboard", 48 | "": "stream all variables", 49 | "--visited --activity": "stream only visited variables and access details", 50 | "odometry": "stream a single variable", 51 | "odometry.pose.pose.position": "stream only a single field within a variable" 52 | } 53 | script_name = "py-trees-blackboard-watcher" 54 | 55 | if formatted_for_sphinx: 56 | # for sphinx documentation (doesn't like raw text) 57 | s = short 58 | s += long 59 | s += "\n" 60 | s += "**Examples:**\n\n" 61 | s += ".. code-block:: bash\n" 62 | s += " \n" 63 | for command, comment in examples.items(): 64 | s += " # {}\n".format(comment) 65 | s += " $ " + script_name + " {}\n".format(command) 66 | s += "\n" 67 | else: 68 | banner_line = console.green + "*" * 79 + "\n" + console.reset 69 | s = "\n" 70 | s += banner_line 71 | s += console.bold_white + "Blackboard Watcher".center(79) + "\n" + console.reset 72 | s += banner_line 73 | s += "\n" 74 | s += short 75 | s += long 76 | s += "\n" 77 | s += console.bold + "Examples" + console.reset + "\n\n" 78 | for command, comment in examples.items(): 79 | s += " # {}\n".format(comment) 80 | s += " $ " + console.cyan + script_name + console.yellow + " {}\n".format(command) + console.reset 81 | s += "\n\n" 82 | s += banner_line 83 | return s 84 | 85 | 86 | def epilog(formatted_for_sphinx): 87 | if formatted_for_sphinx: 88 | return None 89 | else: 90 | return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset 91 | 92 | 93 | def command_line_argument_parser(formatted_for_sphinx=True): 94 | # formatted_for_sphinx is an ugly hack to make sure sphinx does not pick up the colour codes. 95 | # works only by assuming that the only callee who calls it without setting the arg is sphinx's argparse 96 | parser = argparse.ArgumentParser(description=description(formatted_for_sphinx), 97 | epilog=epilog(formatted_for_sphinx), 98 | formatter_class=argparse.RawDescriptionHelpFormatter, 99 | ) 100 | parser.add_argument('-l', '--list', action='store_true', default=None, help='list the blackboard variable names') 101 | parser.add_argument('-a', '--activity', action='store_true', help='include the logged activity stream for recent changes') 102 | parser.add_argument('-v', '--visited', action='store_true', help="filter selected keys from those associated with behaviours on the most recent tick's visited path") 103 | parser.add_argument('-n', '--namespace', nargs='?', default=None, help='namespace of blackboard services (if there should be more than one blackboard)') 104 | parser.add_argument('variables', nargs=argparse.REMAINDER, default=list(), help='space separated list of blackboard variable names (may be nested) to watch') 105 | return parser 106 | 107 | 108 | def pretty_print_variables(variables): 109 | s = "\n" 110 | s += console.bold + console.cyan + "Blackboard Variables:" + console.reset + console.yellow + "\n" 111 | for variable in variables: 112 | variable = variable.split('.') 113 | if len(variable) > 1: 114 | sep = "." 115 | else: 116 | sep = "" 117 | s += " " * len(variable) + sep + variable[-1] + "\n" 118 | s += console.reset 119 | print("{}".format(s)) 120 | 121 | 122 | ############################################################################## 123 | # Main 124 | ############################################################################## 125 | 126 | 127 | def main(command_line_args=sys.argv[1:]): 128 | """ 129 | Entry point for the blackboard watcher script. 130 | """ 131 | # command_line_args = rclpy.utilities.remove_ros_args(command_line_args)[1:] 132 | parser = command_line_argument_parser(formatted_for_sphinx=False) 133 | args = parser.parse_args(command_line_args) 134 | 135 | rclpy.init(args=None) 136 | blackboard_watcher = py_trees_ros.blackboard.BlackboardWatcher( 137 | namespace_hint=args.namespace 138 | ) 139 | subscription = None 140 | #################### 141 | # Setup 142 | #################### 143 | try: 144 | blackboard_watcher.setup(timeout_sec=2.0) 145 | # setup discovery fails 146 | except py_trees_ros.exceptions.NotFoundError as e: 147 | print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset) 148 | sys.exit(1) 149 | # setup discovery finds duplicates 150 | except py_trees_ros.exceptions.MultipleFoundError as e: 151 | print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset) 152 | if args.namespace is None: 153 | print(console.red + "\nERROR: select one with the --namespace argument\n" + console.reset) 154 | else: 155 | print(console.red + "\nERROR: but none matching the requested '{}'\n".format(args.namespace) + console.reset) 156 | sys.exit(1) 157 | 158 | #################### 159 | # Execute 160 | #################### 161 | result = 0 162 | try: 163 | if args.list: 164 | request, client = blackboard_watcher.create_service_client('list') 165 | future = client.call_async(request) 166 | rclpy.spin_until_future_complete(blackboard_watcher.node, future) 167 | if future.result() is None: 168 | raise py_trees_ros.exceptions.ServiceError( 169 | "service call failed [{}]".format(future.exception()) 170 | ) 171 | pretty_print_variables(future.result().variables) 172 | else: 173 | # request connection 174 | request, client = blackboard_watcher.create_service_client('open') 175 | request.variables = [variable.strip(',[]') for variable in args.variables] 176 | request.filter_on_visited_path = args.visited 177 | request.with_activity_stream = args.activity 178 | future = client.call_async(request) 179 | rclpy.spin_until_future_complete(blackboard_watcher.node, future) 180 | response = future.result() 181 | blackboard_watcher.node.destroy_client(client) 182 | # connect 183 | watcher_topic_name = response.topic 184 | blackboard_watcher.node.get_logger().info( 185 | "creating subscription [{}]".format(watcher_topic_name) 186 | ) 187 | subscription = blackboard_watcher.node.create_subscription( 188 | msg_type=std_msgs.String, 189 | topic=watcher_topic_name, 190 | callback=blackboard_watcher.echo_blackboard_contents, 191 | qos_profile=py_trees_ros.utilities.qos_profile_unlatched() 192 | ) 193 | # stream 194 | try: 195 | rclpy.spin(blackboard_watcher.node) 196 | except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): 197 | pass 198 | finally: 199 | # no pre-shutdown hooks from fumble 200 | # https://github.com/ros2/rclpy/issues/1077 201 | # instead, letting the blackboard clean up it's own blackboard views 202 | # when the subscriber count goes to zero 203 | # https://github.com/splintered-reality/py_trees_ros/issues/185 204 | pass 205 | # close connection 206 | # request, client = blackboard_watcher.create_service_client('close') 207 | # request.topic_name = watcher_topic_name 208 | # future = client.call_async(request) 209 | # rclpy.spin_until_future_complete(blackboard_watcher.node, future) 210 | # if future.result() is None: 211 | # raise py_trees_ros.exceptions.ServiceError( 212 | # "service call to close connection failed [{}]".format(future.exception()) 213 | # ) 214 | 215 | # connection problems 216 | except (py_trees_ros.exceptions.NotReadyError, 217 | py_trees_ros.exceptions.ServiceError, 218 | py_trees_ros.exceptions.TimedOutError) as e: 219 | print(console.red + "\nERROR: {}".format(str(e)) + console.reset) 220 | result = 1 221 | finally: 222 | if subscription is not None: 223 | blackboard_watcher.node.destroy_subscription(subscription) 224 | blackboard_watcher.shutdown() 225 | rclpy.try_shutdown() 226 | sys.exit(result) 227 | -------------------------------------------------------------------------------- /py_trees_ros/programs/echo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | .. argparse:: 13 | :module: py_trees_ros.programs.latched_echo 14 | :func: command_line_argument_parser 15 | :prog: py-trees-latched-echo 16 | 17 | Example interaction with the services of a :class:`Blackboard Exchange `: 18 | 19 | .. image:: images/watcher.gif 20 | 21 | .. todo:: switch to using py_tree_ros' robust find_topic call 22 | """ 23 | 24 | ############################################################################## 25 | # Imports 26 | ############################################################################## 27 | 28 | import argparse 29 | import os 30 | import py_trees.console as console 31 | import py_trees_ros.utilities 32 | import rclpy 33 | import ros2topic.api 34 | import ros2cli.node.strategy 35 | import sys 36 | import time 37 | 38 | ############################################################################## 39 | # Module Constants 40 | ############################################################################## 41 | 42 | DEFAULT_TRUNCATE_LENGTH = 128 43 | 44 | ############################################################################## 45 | # Argparse 46 | ############################################################################## 47 | 48 | 49 | def description(): 50 | short = "ROS2 topic echo with latched option for topics\n" 51 | examples = ['/foo/bar'] 52 | script_name = "py-trees-echo" 53 | banner_line = console.green + "*" * 79 + "\n" + console.reset 54 | 55 | s = "\n" 56 | s += banner_line 57 | s += console.bold_white + "Echo".center(79) + "\n" + console.reset 58 | s += banner_line 59 | s += "\n" 60 | s += short 61 | s += "\n" 62 | s += console.bold + "Examples" + console.reset + "\n\n" 63 | s += '\n'.join([" $ " + console.cyan + script_name + console.yellow + " {0}".format(example_args) + console.reset for example_args in examples]) 64 | s += "\n\n" 65 | s += banner_line 66 | return s 67 | 68 | 69 | def epilog(): 70 | return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset 71 | 72 | 73 | def command_line_argument_parser(): 74 | # formatted_for_sphinx is an ugly hack to make sure sphinx does not pick up the colour codes. 75 | # works only by assuming that the only callee who calls it without setting the arg is sphinx's argparse 76 | parser = argparse.ArgumentParser(description=description(), 77 | epilog=epilog(), 78 | formatter_class=argparse.RawDescriptionHelpFormatter, 79 | ) 80 | parser.add_argument('-l', '--latched', action='store_true', help='connect to a latched publisher') 81 | parser.add_argument( 82 | 'topic_name', 83 | help="Name of the ROS topic to listen to (e.g. '/chatter')" 84 | ) 85 | parser.add_argument( 86 | 'message_type', nargs='?', 87 | help="Type of the ROS message (e.g. 'std_msgs/String')" 88 | ) 89 | return parser 90 | 91 | 92 | ############################################################################## 93 | # Helpers 94 | ############################################################################## 95 | 96 | 97 | def create_subscription(node, latched, topic_name, message_type, callback): 98 | if message_type is None: 99 | 100 | for unused_i in range(0, 10): 101 | topic_names_and_types = ros2topic.api.get_topic_names_and_types( 102 | node=node, include_hidden_topics=True 103 | ) 104 | # for n, t in topic_names_and_types: 105 | # print("Name: %s" % n) 106 | # print("Topic: %s" % t) 107 | try: 108 | expanded_name = rclpy.expand_topic_name.expand_topic_name( 109 | topic_name, 110 | node.get_name(), 111 | node.get_namespace() 112 | ) 113 | except ValueError as e: 114 | raise RuntimeError(e) 115 | try: 116 | rclpy.validate_full_topic_name.validate_full_topic_name(expanded_name) 117 | except rclpy.exceptions.InvalidTopicNameException as e: 118 | raise RuntimeError(e) 119 | for n, t in topic_names_and_types: 120 | if n == expanded_name: 121 | if len(t) > 1: 122 | raise RuntimeError( 123 | "Cannot echo topic '%s', as it contains more than one type: [%s]" % 124 | (topic_name, ', '.join(t)) 125 | ) 126 | message_type = t[0] 127 | break 128 | time.sleep(0.1) 129 | if message_type is None: 130 | raise RuntimeError('Could not determine the type for the passed topic') 131 | 132 | msg_module = ros2topic.api.import_message_type(topic_name, message_type) 133 | qos_profile = py_trees_ros.utilities.qos_profile_latched() if latched else py_trees_ros.utilities.qos_profile_unlatched() 134 | return node.create_subscription( 135 | msg_module, 136 | topic_name, 137 | callback, 138 | qos_profile=qos_profile 139 | ) 140 | 141 | 142 | def echo(msg): 143 | print("Type: {}".format(type(msg))) 144 | print("{}".format(msg)) 145 | 146 | 147 | ############################################################################## 148 | # Main 149 | ############################################################################## 150 | 151 | 152 | def main(): 153 | """ 154 | Entry point for the latched echo script. 155 | """ 156 | parser = command_line_argument_parser() 157 | args = parser.parse_args() 158 | 159 | rclpy.init() # picks up sys.argv automagically internally 160 | node = rclpy.create_node("latched_echo" + "_" + str(os.getpid())) 161 | 162 | # timeout_reached = False 163 | # 164 | # def timer_callback(): 165 | # nonlocal timeout_reached 166 | # timeout_reached = True 167 | 168 | # timer = node.create_timer(0.5, timer_callback) 169 | # while not timeout_reached: 170 | # rclpy.spin_once(node) 171 | # node.destroy_timer(timer) 172 | result = 0 173 | try: 174 | unused_subscriber = create_subscription(node, args.latched, args.topic_name, args.message_type, echo) 175 | rclpy.spin(node) 176 | except RuntimeError as e: 177 | console.logerror(str(e)) 178 | node.destroy_node() 179 | result = 1 180 | except KeyboardInterrupt: 181 | pass 182 | node.destroy_node() 183 | sys.exit(result) 184 | -------------------------------------------------------------------------------- /py_trees_ros/programs/multi_talker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | ############################################################################## 5 | # Imports 6 | ############################################################################## 7 | 8 | import rclpy 9 | 10 | from std_msgs.msg import String 11 | 12 | ############################################################################## 13 | # Main 14 | ############################################################################## 15 | 16 | 17 | def main(args=None): 18 | i = 0 19 | timer_period = 0.5 # seconds 20 | 21 | rclpy.init(args=args) 22 | node = rclpy.create_node('multi_talker') 23 | 24 | publisher = node.create_publisher(String, 'topic') 25 | 26 | timeout_reached = False 27 | 28 | def timer_callback(): 29 | nonlocal i 30 | nonlocal timeout_reached 31 | message = 'Hello World: %d' % i 32 | i += 1 33 | node.get_logger().info('"%s"' % message) 34 | timeout_reached = True 35 | 36 | timer = node.create_timer(timer_period, timer_callback) 37 | 38 | while not timeout_reached: 39 | rclpy.spin_once(node) 40 | 41 | node.destroy_timer(timer) 42 | node.destroy_publisher(publisher) 43 | timeout_reached = False 44 | print("Publisher destroyed") 45 | 46 | publisher2 = node.create_publisher(String, 'topic2') 47 | 48 | def timer2_callback(): 49 | nonlocal i 50 | msg = String() 51 | msg.data = 'Hello Again: %d' % i 52 | i += 1 53 | node.get_logger().info('Publishing: "%s"' % msg.data) 54 | publisher2.publish(msg) 55 | 56 | timer2 = node.create_timer(timer_period, timer2_callback) 57 | 58 | try: 59 | rclpy.spin(node) 60 | except KeyboardInterrupt: 61 | pass 62 | 63 | print("Cancel timer") 64 | print("Destroy Timer") 65 | timer2.cancel() 66 | node.destroy_timer(timer2) 67 | node.destroy_node() 68 | rclpy.shutdown() 69 | -------------------------------------------------------------------------------- /py_trees_ros/programs/tree_watcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # License: BSD 4 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 5 | # 6 | ############################################################################## 7 | # Documentation 8 | ############################################################################## 9 | 10 | """ 11 | .. argparse:: 12 | :module: py_trees_ros.programs.tree_watcher 13 | :func: command_line_argument_parser 14 | :prog: py-trees-tree-watcher 15 | 16 | Command line utility that introspects on a running 17 | :class:`~py_trees_ros.trees.BehaviourTree` instance over it's snapshot 18 | stream interfaces. Use to visualise the tree as a dot graph or 19 | track tree changes, timing statistics and blackboard variables visited 20 | by the tree on each tick. 21 | 22 | .. image:: images/tree-watcher.gif 23 | 24 | """ 25 | 26 | ############################################################################## 27 | # Imports 28 | ############################################################################## 29 | 30 | import argparse 31 | import py_trees 32 | import py_trees.console as console 33 | import py_trees_ros 34 | import rclpy 35 | import sys 36 | 37 | ############################################################################## 38 | # Classes 39 | ############################################################################## 40 | 41 | 42 | def description(formatted_for_sphinx): 43 | short = "Open up a window onto the behaviour tree!\n" 44 | long = ("\nRender a oneshot snapshot of the tree as a dot graph, or\n" 45 | "stream it and it's state continuously as unicode art on your console.\n" 46 | "This utility automatically discovers the running tree and opens\n" 47 | "interfaces to that, but if there is more than one tree executing\n" 48 | "use the namespace argument to differentiate between trees.\n" 49 | ) 50 | examples = { 51 | "--dot-graph": "render the tree as a dot graph (does not include runtime information)", 52 | "/tree/snapshots": "connect to an existing snapshot stream (e.g. the default, if it is enabled)", 53 | "": "open and connect to a snapshot stream, visualise the tree graph and it's changes only", 54 | "-b": "open a snapshot stream and include visited blackboard variables", 55 | "-a": "open a snapshot stream and include blackboard access details (activity)", 56 | "-s": "open a snapshot stream and include timing statistics", 57 | } 58 | script_name = "py-trees-tree-watcher" 59 | 60 | if formatted_for_sphinx: 61 | # for sphinx documentation (doesn't like raw text) 62 | s = short 63 | s += long 64 | s += "\n" 65 | s += "**Examples:**\n\n" 66 | s += ".. code-block:: bash\n" 67 | s += " \n" 68 | for command, comment in examples.items(): 69 | s += " # {}\n".format(comment) 70 | s += " $ " + script_name + " {}\n".format(command) 71 | s += "\n" 72 | else: 73 | banner_line = console.green + "*" * 79 + "\n" + console.reset 74 | s = "\n" 75 | s += banner_line 76 | s += console.bold_white + "Tree Watcher".center(79) + "\n" + console.reset 77 | s += banner_line 78 | s += "\n" 79 | s += short 80 | s += long 81 | s += "\n" 82 | s += console.bold + "Examples" + console.reset + "\n\n" 83 | for command, comment in examples.items(): 84 | s += " # {}\n".format(comment) 85 | s += " $ " + console.cyan + script_name + console.yellow + " {}\n".format(command) + console.reset 86 | s += "\n\n" 87 | s += banner_line 88 | return s 89 | 90 | 91 | def epilog(formatted_for_sphinx): 92 | if formatted_for_sphinx: 93 | return None 94 | else: 95 | return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset 96 | 97 | 98 | def command_line_argument_parser(formatted_for_sphinx=True): 99 | # formatted_for_sphinx is an ugly hack to make sure sphinx does not pick up the colour codes. 100 | # works only by assuming that the only callee who calls it without setting the arg is sphinx's argparse 101 | parser = argparse.ArgumentParser(description=description(formatted_for_sphinx), 102 | epilog=epilog(formatted_for_sphinx), 103 | formatter_class=argparse.RawDescriptionHelpFormatter, 104 | ) 105 | # common arguments 106 | parser.add_argument('topic_name', nargs='?', default=None, help='snapshot stream to connect to, will create a temporary stream if none specified') 107 | parser.add_argument('-n', '--namespace-hint', nargs='?', const=None, default=None, help='namespace hint snapshot stream services (if there should be more than one tree)') 108 | parser.add_argument('-a', '--blackboard-activity', action='store_true', help="show logged activity stream (streaming mode only)") 109 | parser.add_argument('-b', '--blackboard-data', action='store_true', help="show visited path variables (streaming mode only)") 110 | parser.add_argument('-s', '--statistics', action='store_true', help="show tick timing statistics (streaming mode only)") 111 | # don't use 'required=True' here since it forces the user to expclitly type out one option 112 | group = parser.add_mutually_exclusive_group() 113 | group.add_argument( 114 | '--snapshots', 115 | dest='mode', 116 | action='store_const', 117 | const=py_trees_ros.trees.WatcherMode.SNAPSHOTS, 118 | help='render ascii/unicode snapshots from a snapshot stream') 119 | group.add_argument( 120 | '--dot-graph', 121 | dest='mode', 122 | action='store_const', 123 | const=py_trees_ros.trees.WatcherMode.DOT_GRAPH, 124 | help='render the tree as a dot graph') 125 | return parser 126 | 127 | 128 | def pretty_print_variables(variables): 129 | s = "\n" 130 | s += console.bold + console.cyan + "Blackboard Variables:" + console.reset + console.yellow + "\n" 131 | for variable in variables: 132 | variable = variable.split('/') 133 | if len(variable) > 1: 134 | sep = "/" 135 | else: 136 | sep = "" 137 | s += " " * len(variable) + sep + variable[-1] + "\n" 138 | s += console.reset 139 | print("{}".format(s)) 140 | 141 | 142 | def echo_blackboard_contents(contents): 143 | """ 144 | Args: 145 | contents (:obj:`str`): blackboard contents 146 | 147 | .. note:: 148 | The string comes pre-formatted with bash color identifiers and newlines. 149 | This is currently not especially good for anything other than debugging. 150 | """ 151 | print("{}".format(contents)) 152 | 153 | ############################################################################## 154 | # Main 155 | ############################################################################## 156 | 157 | 158 | def main(): 159 | """ 160 | Entry point for the tree watcher script. 161 | """ 162 | #################### 163 | # Arg Parsing 164 | #################### 165 | 166 | # command_line_args = rclpy.utilities.remove_ros_args(command_line_args)[1:] 167 | command_line_args = None 168 | parser = command_line_argument_parser(formatted_for_sphinx=False) 169 | args = parser.parse_args(command_line_args) 170 | 171 | # mode is None if the user didn't specify any option in the exclusive group 172 | if args.mode is None: 173 | args.mode = py_trees_ros.trees.WatcherMode.SNAPSHOTS 174 | args.snapshot_period = 2.0 if (args.statistics or args.blackboard_data or args.blackboard_activity) else py_trees.common.Duration.INFINITE.value 175 | tree_watcher = py_trees_ros.trees.Watcher( 176 | namespace_hint=args.namespace_hint, 177 | topic_name=args.topic_name, 178 | parameters=py_trees_ros.trees.SnapshotStream.Parameters( 179 | blackboard_data=args.blackboard_data, 180 | blackboard_activity=args.blackboard_activity, 181 | snapshot_period=args.snapshot_period 182 | ), 183 | mode=args.mode, 184 | statistics=args.statistics, 185 | ) 186 | 187 | #################### 188 | # Setup 189 | #################### 190 | rclpy.init(args=None) 191 | try: 192 | tree_watcher.setup(timeout_sec=5.0) 193 | # setup discovery fails 194 | except py_trees_ros.exceptions.NotFoundError as e: 195 | print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset) 196 | sys.exit(1) 197 | # setup discovery finds duplicates 198 | except py_trees_ros.exceptions.MultipleFoundError as e: 199 | print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset) 200 | if args.namespace is None: 201 | print(console.red + "\nERROR: select one with the --namespace argument\n" + console.reset) 202 | else: 203 | print(console.red + "\nERROR: but none matching the requested '{}'\n".format(args.namespace) + console.reset) 204 | sys.exit(1) 205 | except py_trees_ros.exceptions.TimedOutError as e: 206 | print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset) 207 | sys.exit(1) 208 | 209 | #################### 210 | # Execute 211 | #################### 212 | executor = rclpy.executors.SingleThreadedExecutor() 213 | executor.add_node(node=tree_watcher.node) 214 | try: 215 | while True: 216 | if not rclpy.ok(): 217 | break 218 | if tree_watcher.done: 219 | if tree_watcher.xdot_process is None: 220 | # no xdot found on the system, just break out and finish 221 | break 222 | elif tree_watcher.xdot_process.poll() is not None: 223 | # xdot running, wait for it to terminate 224 | break 225 | executor.spin_once(timeout_sec=0.1) 226 | except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): 227 | pass 228 | finally: 229 | if tree_watcher.xdot_process is not None: 230 | if tree_watcher.xdot_process.poll() is not None: 231 | tree_watcher.xdot_process.terminate() 232 | tree_watcher.shutdown() 233 | rclpy.try_shutdown() 234 | -------------------------------------------------------------------------------- /py_trees_ros/publishers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Description 9 | ############################################################################## 10 | 11 | """ 12 | Convenience behaviours for publishing ROS messages. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import typing 20 | 21 | import py_trees 22 | import rclpy.qos 23 | 24 | ############################################################################## 25 | # Behaviours 26 | ############################################################################## 27 | 28 | 29 | class FromBlackboard(py_trees.behaviour.Behaviour): 30 | """ 31 | This behaviour looks up the blackboard for content to publish ... 32 | and publishes it. 33 | 34 | This is a non-blocking behaviour - if there is no data yet on 35 | the blackboard it will tick with :data:`~py_trees.common.Status.FAILURE`, 36 | otherwise :data:`~py_trees.common.Status.SUCCESS`. 37 | 38 | To convert it to a blocking behaviour, simply use with the 39 | :class:`py_trees.behaviours.WaitForBlackboardVariable`. e.g. 40 | 41 | .. code-block:: python 42 | 43 | sequence = py_trees.composites.Sequence(name="Sequence", memory=True) 44 | wait_for_data = py_trees.behaviours.WaitForBlackboardVariable( 45 | name="WaitForData", 46 | variable_name="/my_message" 47 | ) 48 | publisher = py_trees_ros.publishers.FromBlackboard( 49 | topic_name="/foo", 50 | topic_type=std_msgs.msg.Empty 51 | qos_profile=my_qos_profile, 52 | blackboard_variable="/my_message" 53 | ) 54 | sequence.add_children([wait_for_data, publisher]) 55 | 56 | The various set/unset blackboard variable behaviours can also be useful for 57 | setting and unsetting the message to be published (typically elsewhere 58 | in the tree). 59 | 60 | Args: 61 | name: name of the behaviour 62 | topic_name: name of the topic to connect to 63 | topic_type: class of the message type (e.g. :obj:`std_msgs.msg.String`) 64 | qos_profile: qos profile for the subscriber 65 | blackboard_variable: name of the variable on the blackboard (can be nested) 66 | """ 67 | def __init__(self, 68 | name: str, 69 | topic_name: str, 70 | topic_type: typing.Any, 71 | qos_profile: rclpy.qos.QoSProfile, 72 | blackboard_variable: str, 73 | ): 74 | super().__init__(name=name) 75 | self.topic_name = topic_name 76 | self.topic_type = topic_type 77 | self.blackboard = self.attach_blackboard_client(name=self.name) 78 | self.blackboard_variable = blackboard_variable 79 | self.key = blackboard_variable.split('.')[0] # in case it is nested 80 | self.blackboard.register_key( 81 | key=self.key, 82 | access=py_trees.common.Access.READ 83 | ) 84 | self.publisher = None 85 | self.qos_profile = qos_profile 86 | self.node = None 87 | 88 | def setup(self, **kwargs): 89 | """ 90 | Initialises the publisher. 91 | 92 | Args: 93 | **kwargs (:obj:`dict`): distribute arguments to this 94 | behaviour and in turn, all of it's children 95 | 96 | Raises: 97 | KeyError: if a ros2 node isn't passed under the key 'node' in kwargs 98 | """ 99 | try: 100 | self.node = kwargs['node'] 101 | except KeyError as e: 102 | error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.name, self.__class__.__name__) 103 | raise KeyError(error_message) from e # 'direct cause' traceability 104 | self.publisher = self.node.create_publisher( 105 | msg_type=self.topic_type, 106 | topic=self.topic_name, 107 | qos_profile=self.qos_profile 108 | ) 109 | 110 | def update(self): 111 | """ 112 | Publish the specified variable from the blackboard. 113 | 114 | Raises: 115 | TypeError if the blackboard variable is not of the required type 116 | 117 | Returns: 118 | :data:`~py_trees.common.Status.FAILURE` (variable does not exist on the blackboard) or :data:`~py_trees.common.Status.SUCCESS` (published) 119 | """ 120 | self.logger.debug("%s.update()" % self.__class__.__name__) 121 | try: 122 | if isinstance(self.blackboard.get(self.blackboard_variable), self.topic_type): 123 | self.publisher.publish(self.blackboard.get(self.blackboard_variable)) 124 | else: 125 | raise TypeError("{} is not the required type [{}][{}]".format( 126 | self.blackboard_variable, 127 | self.topic_type, 128 | type(self.blackboard.get(self.blackboard_variable))) 129 | ) 130 | self.feedback_message = "published" 131 | return py_trees.common.Status.SUCCESS 132 | except KeyError: 133 | self.feedback_message = "nothing to publish" 134 | return py_trees.common.Status.FAILURE 135 | -------------------------------------------------------------------------------- /py_trees_ros/service_clients.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Behaviours for ROS services. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | from asyncio.tasks import wait_for 20 | import typing 21 | import uuid 22 | 23 | import py_trees 24 | 25 | from . import exceptions 26 | 27 | ############################################################################## 28 | # Behaviours 29 | ############################################################################## 30 | 31 | 32 | class FromBlackboard(py_trees.behaviour.Behaviour): 33 | """ 34 | An service client interface that draws requests from the blackboard. The 35 | lifecycle of this behaviour works as follows: 36 | 37 | * :meth:`initialise`: check blackboard for a request and send 38 | * :meth:`update`: if a request was sent, monitor progress 39 | * :meth:`terminate`: if interrupted while running, send a cancel request 40 | 41 | As a consequence, the status of this behaviour can be interpreted as follows: 42 | 43 | * :data:`~py_trees.common.Status.FAILURE`: no request was found to send, 44 | the server was not ready, or it failed while executing 45 | * :data:`~py_trees.common.Status.RUNNING`: a request was sent and is still 46 | executing 47 | * :data:`~py_trees.common.Status.SUCCESS`: sent request has completed with success 48 | 49 | To block on the arrival of a request on the blackboard, use with the 50 | :class:`py_trees.behaviours.WaitForBlackboardVariable` behaviour. e.g. 51 | 52 | Args: 53 | name: name of the behaviour 54 | service_type: spec type for the service 55 | service_name: where you can find the service 56 | key_request: name of the key for the request on the blackboard 57 | key_response: optional name of the key for the response on the blackboard (default: None) 58 | wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) 59 | 60 | .. note:: 61 | The default setting for timeouts (a negative value) will suit 62 | most use cases. With this setting the behaviour will periodically check and 63 | issue a warning if the server can't be found. Actually aborting the setup can 64 | usually be left up to the behaviour tree manager. 65 | """ 66 | def __init__(self, 67 | name: str, 68 | service_type: typing.Any, 69 | service_name: str, 70 | key_request: str, 71 | key_response: typing.Optional[str]=None, 72 | wait_for_server_timeout_sec: float=-3.0 73 | ): 74 | super().__init__(name) 75 | self.service_type = service_type 76 | self.service_name = service_name 77 | self.wait_for_server_timeout_sec = wait_for_server_timeout_sec 78 | self.blackboard = self.attach_blackboard_client(name=self.name) 79 | self.blackboard.register_key( 80 | key="request", 81 | access=py_trees.common.Access.READ, 82 | # make sure to namespace it if not already 83 | remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key_request) 84 | ) 85 | self.write_response_to_blackboard = key_response is not None 86 | if self.write_response_to_blackboard: 87 | self.blackboard.register_key( 88 | key="response", 89 | access=py_trees.common.Access.WRITE, 90 | # make sure to namespace it if not already 91 | remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key_response) 92 | ) 93 | 94 | self.node = None 95 | self.service_client = None 96 | 97 | def setup(self, **kwargs): 98 | """ 99 | Setup the service client and ensure it is available. 100 | 101 | Args: 102 | **kwargs (:obj:`dict`): distribute arguments to this 103 | behaviour and in turn, all of it's children 104 | 105 | Raises: 106 | :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs 107 | :class:`~py_trees_ros.exceptions.TimedOutError`: if the service server could not be found 108 | """ 109 | self.logger.debug("{}.setup()".format(self.qualified_name)) 110 | try: 111 | self.node = kwargs['node'] 112 | except KeyError as e: 113 | error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.qualified_name) 114 | raise KeyError(error_message) from e # 'direct cause' traceability 115 | 116 | self.service_client = self.node.create_client(srv_type=self.service_type, srv_name=self.service_name) 117 | 118 | result = None 119 | if self.wait_for_server_timeout_sec > 0.0: 120 | result = self.service_client.wait_for_service(timeout_sec=self.wait_for_server_timeout_sec) 121 | elif self.wait_for_server_timeout_sec == 0.0: 122 | result = True # don't wait and don't check if the server is ready 123 | else: 124 | iterations = 0 125 | period_sec = -1.0*self.wait_for_server_timeout_sec 126 | while not result: 127 | iterations += 1 128 | result = self.service_client.wait_for_service(timeout_sec=period_sec) 129 | if not result: 130 | self.node.get_logger().warning( 131 | "waiting for service server ... [{}s][{}][{}]".format( 132 | iterations * period_sec, 133 | self.node.resolve_service_name(self.service_name), 134 | self.qualified_name 135 | ) 136 | ) 137 | if not result: 138 | self.feedback_message = "timed out waiting for the server [{}]".format( 139 | self.node.resolve_service_name(self.service_name) 140 | ) 141 | self.node.get_logger().error("{}[{}]".format(self.feedback_message, self.qualified_name)) 142 | raise exceptions.TimedOutError(self.feedback_message) 143 | else: 144 | self.feedback_message = "... connected to service server [{}]".format( 145 | self.node.resolve_service_name(self.service_name) 146 | ) 147 | self.node.get_logger().info("{}[{}]".format(self.feedback_message, self.qualified_name)) 148 | 149 | def initialise(self): 150 | """ 151 | Reset the internal variables and kick off a new request request. 152 | """ 153 | self.logger.debug("{}.initialise()".format(self.qualified_name)) 154 | 155 | # initialise some temporary variables 156 | self.service_future = None 157 | 158 | try: 159 | if self.service_client.service_is_ready(): 160 | self.service_future = self.service_client.call_async(self.blackboard.request) 161 | except (KeyError, TypeError): 162 | pass # self.service_future will be None, check on that 163 | 164 | def update(self): 165 | """ 166 | Check only to see whether the underlying service server has 167 | succeeded, is running, or has cancelled/aborted for some reason and 168 | map these to the usual behaviour return states. 169 | 170 | Returns: 171 | :class:`py_trees.common.Status` 172 | """ 173 | self.logger.debug("{}.update()".format(self.qualified_name)) 174 | 175 | if self.service_future is None: 176 | # either there was no request on the blackboard, the request's type 177 | # was wrong, or the service server wasn't ready 178 | return py_trees.common.Status.FAILURE 179 | elif not self.service_future.done(): 180 | # service has been called but hasn't yet returned a result 181 | return py_trees.common.Status.RUNNING 182 | else: 183 | # service has succeeded; get the result 184 | self.response = self.service_future.result() 185 | if self.write_response_to_blackboard: 186 | self.blackboard.response = self.response 187 | return py_trees.common.Status.SUCCESS 188 | 189 | def terminate(self, new_status: py_trees.common.Status): 190 | """ 191 | If running and the current request has not already succeeded, cancel it. 192 | 193 | Args: 194 | new_status: the behaviour is transitioning to this new status 195 | """ 196 | self.logger.debug( 197 | "{}.terminate({})".format( 198 | self.qualified_name, 199 | "{}->{}".format(self.status, new_status) if self.status != new_status else "{}".format(new_status) 200 | ) 201 | ) 202 | if (self.service_future is not None) and (not self.service_future.done()): 203 | self.service_client.remove_pending_request(self.service_future) 204 | 205 | def shutdown(self): 206 | """ 207 | Clean up the service client when shutting down. 208 | """ 209 | self.service_client.destroy() 210 | 211 | 212 | class FromConstant(FromBlackboard): 213 | """ 214 | Convenience version of the service client that only ever sends the 215 | same goal. 216 | 217 | .. see-also: :class:`py_trees_ros.service_clients.FromBlackboard` 218 | 219 | Args: 220 | name: name of the behaviour 221 | name: name of the behaviour 222 | service_type: spec type for the service 223 | service_name: where you can find the service 224 | service_request: the request to send 225 | key_response: optional name of the key for the response on the blackboard (default: None) 226 | wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) 227 | 228 | .. note:: 229 | The default setting for timeouts (a negative value) will suit 230 | most use cases. With this setting the behaviour will periodically check and 231 | issue a warning if the server can't be found. Actually aborting the setup can 232 | usually be left up to the behaviour tree manager. 233 | """ 234 | def __init__(self, 235 | name: str, 236 | service_type: typing.Any, 237 | service_name: str, 238 | service_request: typing.Any, 239 | key_response: typing.Optional[str]=None, 240 | wait_for_server_timeout_sec: float=-3.0 241 | ): 242 | unique_id = uuid.uuid4() 243 | key_request = "/goal_" + str(unique_id) 244 | super().__init__( 245 | service_type=service_type, 246 | service_name=service_name, 247 | key_request=key_request, 248 | key_response=key_response, 249 | name=name, 250 | wait_for_server_timeout_sec=wait_for_server_timeout_sec 251 | ) 252 | # parent already instantiated a blackboard client 253 | self.blackboard.register_key( 254 | key=key_request, 255 | access=py_trees.common.Access.WRITE, 256 | ) 257 | self.blackboard.set(name=key_request, value=service_request) 258 | -------------------------------------------------------------------------------- /py_trees_ros/transforms.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Description 9 | ############################################################################## 10 | 11 | """ 12 | Various behaviours that enable common interactions with ROS transforms. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import py_trees 20 | import rclpy.qos 21 | import tf2_ros 22 | 23 | import py_trees.console as console 24 | 25 | import geometry_msgs.msg as geometry_msgs 26 | 27 | ############################################################################## 28 | # Behaviours 29 | ############################################################################## 30 | 31 | 32 | class FromBlackboard(py_trees.behaviour.Behaviour): 33 | """ 34 | Broadcast a transform from the blackboard using the 35 | transform broadcaster mechanisms. 36 | 37 | If no it fails to find a geometry_msgs.Transform 38 | object on the blackboard, or the blackboard variable is None, 39 | this behaviour will update with status 40 | :attr:`~py_trees.common.Status.FAILURE`. 41 | 42 | Args: 43 | variable_name: name of the transform variable on the blackboard 44 | target_frame: name of the frame to transform into 45 | source_frame: name of the input frame 46 | static: designate whether it is a static transform or otherwise 47 | qos_profile: qos profile for the non-static broadcaster 48 | static_qos_profile: qos profile for the static broadcaster (default: use tf2_ros' defaults) 49 | name: name of the behaviour 50 | """ 51 | def __init__( 52 | self, 53 | name: str, 54 | variable_name: str, 55 | target_frame: str, 56 | source_frame: str, 57 | static: bool, 58 | qos_profile: rclpy.qos.QoSProfile, 59 | static_qos_profile: rclpy.qos.QoSProfile=None, 60 | ): 61 | super().__init__(name=name) 62 | self.blackboard = self.attach_blackboard_client(name) 63 | self.variable_name = variable_name 64 | self.target_frame = target_frame 65 | self.source_frame = source_frame 66 | self.static = static 67 | self.qos_profile = qos_profile 68 | self.static_qos_profile = static_qos_profile 69 | 70 | self.blackboard.register_key( 71 | key=self.variable_name, 72 | access=py_trees.common.Access.READ 73 | ) 74 | 75 | def setup(self, **kwargs): 76 | """ 77 | Initialises the transform broadcaster. 78 | 79 | Args: 80 | **kwargs (:obj:`dict`): distribute arguments to this 81 | behaviour and in turn, all of it's children 82 | 83 | Raises: 84 | KeyError: if a ros2 node isn't passed under the key 'node' in kwargs 85 | """ 86 | try: 87 | self.node = kwargs['node'] 88 | except KeyError as e: 89 | error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.name, self.__class__.__name__) 90 | raise KeyError(error_message) from e # 'direct cause' traceability 91 | if self.static: 92 | self.broadcaster = tf2_ros.StaticTransformBroadcaster( 93 | node=self.node, 94 | qos=self.static_qos_profile 95 | ) 96 | else: 97 | self.broadcaster = tf2_ros.TransformBroadcaster( 98 | node=self.node, 99 | qos=self.qos_profile 100 | ) 101 | 102 | def update(self): 103 | """ 104 | Retrieves the transform from the blackboard, stamps it 105 | and subsequently broadcasts it. 106 | 107 | Raises: 108 | TypeError: if the blackboard variable is of the wrong type. 109 | """ 110 | try: 111 | transform = self.blackboard.get(self.variable_name) 112 | except KeyError: 113 | self.feedback_message = "no transform to broadcast" 114 | return py_trees.common.Status.FAILURE 115 | if transform is None: 116 | self.feedback_message = "no transform to broadcast" 117 | return py_trees.common.Status.FAILURE 118 | if type(transform) != geometry_msgs.Transform: 119 | raise TypeError("'{}' is not of type geometry_msgs/Transform".format(self.variable_name)) 120 | self.feedback_message = "transform sent" 121 | transform_stamped = geometry_msgs.TransformStamped() 122 | transform_stamped.header.stamp = rclpy.clock.Clock().now().to_msg() 123 | transform_stamped.header.frame_id = self.source_frame 124 | transform_stamped.child_frame_id = self.target_frame 125 | transform_stamped.transform = transform 126 | # print(console.green + "Send: {}".format(transform_stamped) + console.reset) 127 | self.broadcaster.sendTransform(transform_stamped) 128 | return py_trees.common.Status.SUCCESS 129 | 130 | 131 | class ToBlackboard(py_trees.behaviour.Behaviour): 132 | """ 133 | Blocking behaviour that looks for a transform and writes 134 | it to a variable on the blackboard. 135 | 136 | If it fails to find a transform immediately, it will update 137 | with status :attr:`~py_trees.common.Status.RUNNING` and write 138 | 'None' to the blackboard. 139 | 140 | .. tip:: 141 | 142 | To ensure consistent decision making, use this behaviour 143 | up-front in your tree's tick to record a transform that 144 | can be locked in for the remainder of the tree tick. 145 | 146 | **Usage Patterns** 147 | 148 | * clearing_policy == :attr:`~py_trees.common.ClearingPolicy.ON_INTIALISE` 149 | 150 | Use if you have subsequent behaviours that need to make decisions on 151 | whether the transform was received or not. 152 | 153 | * clearing_policy == :attr:`~py_trees.common.ClearingPolicy.NEVER` 154 | 155 | Never clear the result. Useful for static transforms or if you are doing 156 | your own lookup on the timestamps for any relevant decision making. 157 | 158 | Args: 159 | variable_name: name of the key to write to on the blackboard 160 | target_frame: name of the frame to transform into 161 | source_frame: name of the input frame 162 | qos_profile: qos profile for the non-static subscriber 163 | static_qos_profile: qos profile for the static subscriber (default: use tf2_ros' defaults) 164 | name: name of the behaviour 165 | 166 | Raises: 167 | TypeError: if the clearing policy is neither 168 | :attr:`~py_trees.common.ClearingPolicy.ON_INITIALISE` 169 | or :attr:`~py_trees.common.ClearingPolicy.NEVER` 170 | """ 171 | def __init__( 172 | self, 173 | name: str, 174 | variable_name, 175 | target_frame: str, 176 | source_frame: str, 177 | qos_profile: rclpy.qos.QoSProfile, 178 | static_qos_profile: rclpy.qos.QoSProfile=None, 179 | clearing_policy: py_trees.common.ClearingPolicy=py_trees.common.ClearingPolicy.ON_INITIALISE, 180 | ): 181 | super().__init__(name=name) 182 | self.variable_name = variable_name 183 | self.blackboard = self.attach_blackboard_client(name) 184 | self.blackboard.register_key( 185 | key=self.variable_name, 186 | access=py_trees.common.Access.WRITE 187 | ) 188 | self.target_frame = target_frame 189 | self.source_frame = source_frame 190 | self.qos_profile = qos_profile 191 | self.static_qos_profile = static_qos_profile 192 | self.clearing_policy = clearing_policy 193 | if self.clearing_policy == py_trees.common.ClearingPolicy.ON_SUCCESS: 194 | raise TypeError("ON_SUCCESS is not a valid policy for transforms.ToBlackboard") 195 | self.buffer = tf2_ros.Buffer() 196 | # initialise the blackboard 197 | self.blackboard.set(self.variable_name, None) 198 | 199 | def setup(self, **kwargs): 200 | """ 201 | Initialises the transform listener. 202 | 203 | Args: 204 | **kwargs (:obj:`dict`): distribute arguments to this 205 | behaviour and in turn, all of it's children 206 | 207 | Raises: 208 | KeyError: if a ros2 node isn't passed under the key 'node' in kwargs 209 | """ 210 | try: 211 | self.node = kwargs['node'] 212 | except KeyError as e: 213 | error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.name, self.__class__.__name__) 214 | raise KeyError(error_message) from e # 'direct cause' traceability 215 | self.listener = tf2_ros.TransformListener( 216 | buffer=self.buffer, 217 | node=self.node, 218 | # spin_thread=False, 219 | qos=self.qos_profile, 220 | static_qos=self.static_qos_profile 221 | ) 222 | 223 | def initialise(self): 224 | """ 225 | Clear the blackboard variable (set to 'None') if using the 226 | :attr:`~py_trees.common.ClearingPolicy.ON_INTIALISE` policy. 227 | """ 228 | if self.clearing_policy == py_trees.common.ClearingPolicy.ON_INITIALISE: 229 | self.blackboard.set(self.variable_name, None) 230 | 231 | def update(self): 232 | """ 233 | Checks for the latest transform and posts it to the blackboard 234 | if available. 235 | """ 236 | class get_latest(object): 237 | def __init__(self): 238 | self.nanoseconds = 0 239 | 240 | if self.buffer.can_transform( 241 | target_frame=self.target_frame, 242 | source_frame=self.source_frame, 243 | time=get_latest(), 244 | # timeout=rclpy.duration.Duration(seconds=5) # don't block 245 | ): 246 | stamped_transform = self.buffer.lookup_transform( 247 | target_frame=self.target_frame, 248 | source_frame=self.source_frame, 249 | time=get_latest(), 250 | # timeout=rclpy.duration.Duration(seconds=5) # don't block 251 | ) 252 | self.blackboard.set(self.variable_name, stamped_transform) 253 | self.feedback_message = "transform saved to {}".format(self.variable_name) 254 | return py_trees.common.Status.SUCCESS 255 | else: 256 | self.feedback_message = "waiting for transform".format( 257 | self.target_frame, 258 | self.source_frame 259 | ) 260 | return py_trees.common.Status.RUNNING 261 | -------------------------------------------------------------------------------- /py_trees_ros/utilities.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | Assorted utility functions. 13 | """ 14 | 15 | ############################################################################## 16 | # Imports 17 | ############################################################################## 18 | 19 | import os 20 | import pathlib 21 | 22 | import py_trees_ros_interfaces.msg as py_trees_msgs # noqa 23 | import py_trees_ros_interfaces.srv as py_trees_srvs # noqa 24 | import rclpy 25 | import rclpy.node 26 | import rclpy.qos 27 | import time 28 | import typing 29 | 30 | from . import exceptions 31 | 32 | ############################################################################## 33 | # Methods 34 | ############################################################################## 35 | 36 | 37 | def find_service(node: rclpy.node.Node, 38 | service_type: str, 39 | namespace: str=None, 40 | timeout: float=0.5): 41 | """ 42 | Discover a service of the specified type and if necessary, under the specified 43 | namespace. 44 | 45 | Args: 46 | node (:class:`rclpy.node.Node`): nodes have the discovery methods 47 | service_type (:obj:`str`): primary lookup hint 48 | namespace (:obj:`str`): secondary lookup hint 49 | timeout: immediately post node creation, can take time to discover the graph (sec) 50 | 51 | Returns: 52 | :obj:`str`: fully expanded service name 53 | 54 | Raises: 55 | :class:`~py_trees_ros.exceptions.NotFoundError`: if no services were found 56 | :class:`~py_trees_ros.exceptions.MultipleFoundError`: if multiple services were found 57 | """ 58 | # TODO: follow the pattern of ros2cli to create a node without the need to init 59 | # rcl (might get rid of the magic sleep this way). See: 60 | # https://github.com/ros2/ros2cli/blob/master/ros2service/ros2service/verb/list.py 61 | # https://github.com/ros2/ros2cli/blob/master/ros2cli/ros2cli/node/strategy.py 62 | 63 | loop_period = 0.1 # seconds 64 | clock = rclpy.clock.Clock() 65 | start_time = clock.now() 66 | service_names = [] 67 | while clock.now() - start_time < rclpy.time.Duration(seconds=timeout): 68 | # Returns a list of the form: [('exchange/blackboard', ['std_msgs/String']) 69 | service_names_and_types = node.get_service_names_and_types() 70 | service_names = [name for name, types in service_names_and_types if service_type in types] 71 | if namespace is not None: 72 | service_names = [name for name in service_names if namespace in name] 73 | if service_names: 74 | break 75 | time.sleep(loop_period) 76 | 77 | if not service_names: 78 | raise exceptions.NotFoundError("service not found [type: {}]".format(service_type)) 79 | elif len(service_names) == 1: 80 | return service_names[0] 81 | else: 82 | raise exceptions.MultipleFoundError("multiple services found [type: {}]".format(service_type)) 83 | 84 | 85 | def find_topics( 86 | node: rclpy.node.Node, 87 | topic_type: str, 88 | namespace: str=None, 89 | timeout: float=0.5) -> typing.List[str]: 90 | """ 91 | Discover a topic of the specified type and if necessary, under the specified 92 | namespace. 93 | 94 | Args: 95 | node: nodes have the discovery methods 96 | topic_type: primary lookup hint 97 | namespace: secondary lookup hint 98 | timeout: check every 0.1s until this timeout is reached (can be None -> checks once) 99 | 100 | .. note: Immediately post node creation, it can take some time to discover the graph. 101 | 102 | Returns: 103 | list of fully expanded topic names (can be empty) 104 | """ 105 | # TODO: follow the pattern of ros2cli to create a node without the need to init 106 | # rcl (might get rid of the magic sleep this way). See: 107 | # https://github.com/ros2/ros2cli/blob/master/ros2service/ros2service/verb/list.py 108 | # https://github.com/ros2/ros2cli/blob/master/ros2cli/ros2cli/node/strategy.py 109 | loop_period = 0.1 # seconds 110 | clock = rclpy.clock.Clock() 111 | start_time = clock.now() 112 | topic_names = [] 113 | while True: 114 | # Returns a list of the form: [('exchange/blackboard', ['std_msgs/String']) 115 | topic_names_and_types = node.get_topic_names_and_types() 116 | topic_names = [name for name, types in topic_names_and_types if topic_type in types] 117 | if namespace is not None: 118 | topic_names = [name for name in topic_names if namespace in name] 119 | if topic_names: 120 | break 121 | if timeout is None or (clock.now() - start_time) > rclpy.time.Duration(seconds=timeout): 122 | break 123 | else: 124 | time.sleep(loop_period) 125 | return topic_names 126 | 127 | 128 | def basename(name): 129 | """ 130 | Generate the basename from a ros name. 131 | 132 | Args: 133 | name (:obj:`str`): ros name 134 | 135 | Returns: 136 | :obj:`str`: name stripped up until the last slash or tilde character. 137 | Examples: 138 | 139 | .. code-block:: python 140 | 141 | basename("~dude") 142 | # 'dude' 143 | basename("/gang/dude") 144 | # 'dude' 145 | """ 146 | return name.rsplit('/', 1)[-1].rsplit('~', 1)[-1] 147 | 148 | 149 | def get_py_trees_home(): 150 | """ 151 | Find the default home directory used for logging, bagging and other 152 | esoterica. 153 | """ 154 | # TODO: update with replacement for rospkg.get_ros_home() when it arrives 155 | home = os.path.join(str(pathlib.Path.home()), ".ros2", "py_trees") 156 | return home 157 | 158 | 159 | def qos_profile_latched(): 160 | """ 161 | Convenience retrieval for a latched topic (publisher / subscriber) 162 | """ 163 | return rclpy.qos.QoSProfile( 164 | history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, 165 | depth=1, 166 | durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL, 167 | reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE 168 | ) 169 | 170 | 171 | def qos_profile_unlatched(): 172 | """ 173 | Default profile for an unlatched topic (in py_trees_ros). 174 | """ 175 | return rclpy.qos.QoSProfile( 176 | history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, 177 | depth=1, 178 | durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE, 179 | reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE 180 | ) 181 | 182 | 183 | def resolve_name(node, name): 184 | """ 185 | Convenience function for getting the resolved name (similar to 'publisher.resolved_name' in ROS1). 186 | 187 | Args: 188 | node (:class:`rclpy.node.Node`): the node, namespace it *should* be relevant to 189 | name (obj:`str`): topic or service name 190 | 191 | .. note:: 192 | 193 | This entirely depends on the user providing the relevant node, name pair. 194 | """ 195 | return rclpy.expand_topic_name.expand_topic_name( 196 | name, 197 | node.get_name(), 198 | node.get_namespace() 199 | ) 200 | 201 | 202 | def create_anonymous_node_name(node_name="node") -> str: 203 | """ 204 | Creates an anonoymous node name by adding a suffix created from 205 | a monotonic timestamp, sans the decimal. 206 | 207 | Returns: 208 | :obj:`str`: the unique, anonymous node name 209 | """ 210 | return node_name + "_" + str(time.monotonic()).replace('.', '') 211 | 212 | 213 | ############################################################################## 214 | # Convenience Classes 215 | ############################################################################## 216 | 217 | 218 | class Publishers(object): 219 | """ 220 | Utility class that groups the publishers together in one convenient structure. 221 | 222 | Args: 223 | publisher_details (obj:`tuple`): list of (str, str, msgType, bool, int) tuples representing 224 | (unique_name, topic_name, publisher_type, latched) 225 | specifications for creating publishers 226 | 227 | Examples: 228 | Convert the incoming list of specifications into proper variables of this class. 229 | 230 | .. code-block:: python 231 | 232 | publishers = py_trees.utilities.Publishers( 233 | [ 234 | ('foo', '~/foo', std_msgs.String, True, 5), 235 | ('bar', '/foo/bar', std_msgs.String, False, 5), 236 | ('foobar', '/foo/bar', std_msgs.String, False, 5), 237 | ] 238 | ) 239 | """ 240 | def __init__(self, node, publisher_details, introspection_topic_name="publishers"): 241 | # TODO: check for the correct setting of publisher_details 242 | self.publisher_details_msg = [] 243 | for (name, topic_name, publisher_type, latched) in publisher_details: 244 | if latched: 245 | self.__dict__[name] = node.create_publisher( 246 | msg_type=publisher_type, 247 | topic=topic_name, 248 | qos_profile=qos_profile_latched() 249 | ) 250 | else: 251 | self.__dict__[name] = node.create_publisher( 252 | msg_type=publisher_type, 253 | topic=topic_name, 254 | qos_profile=qos_profile_unlatched() 255 | ) 256 | resolved_name = resolve_name(node, topic_name) 257 | message_type = publisher_type.__class__.__module__.split('.')[0] + "/" + publisher_type.__class__.__name__ 258 | self.publisher_details_msg.append( 259 | py_trees_msgs.PublisherDetails( 260 | topic_name=resolved_name, 261 | message_type=message_type, 262 | latched=latched 263 | ) 264 | ) 265 | 266 | self.introspection_service = node.create_service( 267 | py_trees_srvs.IntrospectPublishers, 268 | "~/introspection/" + introspection_topic_name, 269 | self.introspection_callback 270 | ) 271 | 272 | def introspection_callback(self, unused_request, response): 273 | response.publisher_details = self.publisher_details_msg 274 | return response 275 | 276 | 277 | class Subscribers(object): 278 | """ 279 | Utility class that groups subscribers together in one convenient structure. 280 | 281 | Args: 282 | subscriber_details (obj:`tuple`): list of (str, str, msgType, bool, func) tuples representing 283 | (unique_name, topic_name, subscriber_type, latched, callback) 284 | specifications for creating subscribers 285 | 286 | Examples: 287 | Convert the incoming list of specifications into proper variables of this class. 288 | 289 | .. code-block:: python 290 | 291 | subscribers = py_trees.utilities.Subscribers( 292 | [ 293 | ('foo', '~/foo', std_msgs.String, True, foo), 294 | ('bar', '/foo/bar', std_msgs.String, False, self.foo), 295 | ('foobar', '/foo/bar', std_msgs.String, False, foo.bar), 296 | ] 297 | ) 298 | """ 299 | def __init__(self, node, subscriber_details, introspection_topic_name="subscribers"): 300 | # TODO: check for the correct setting of subscriber_details 301 | self.subscriber_details_msg = [] 302 | for (name, topic_name, subscriber_type, latched, callback) in subscriber_details: 303 | if latched: 304 | self.__dict__[name] = node.create_subscription( 305 | msg_type=subscriber_type, 306 | topic=topic_name, 307 | callback=callback, 308 | qos_profile=qos_profile_latched() 309 | ) 310 | else: 311 | self.__dict__[name] = node.create_subscription( 312 | msg_type=subscriber_type, 313 | topic=topic_name, 314 | callback=callback, 315 | qos_profile=qos_profile_unlatched() 316 | ) 317 | 318 | resolved_name = resolve_name(node, topic_name) 319 | message_type = subscriber_type.__class__.__module__.split('.')[0] + "/" + subscriber_type.__class__.__name__ 320 | self.subscriber_details_msg.append( 321 | py_trees_msgs.SubscriberDetails( 322 | topic_name=resolved_name, 323 | message_type=message_type, 324 | latched=latched 325 | ) 326 | ) 327 | 328 | self.introspection_service = node.create_service( 329 | py_trees_srvs.IntrospectSubscribers, 330 | "~/introspection/" + introspection_topic_name, 331 | self.introspection_callback 332 | ) 333 | 334 | def introspection_callback(self, unused_request, response): 335 | response.subscriber_details = self.subscriber_details_msg 336 | return response 337 | 338 | 339 | class Services(object): 340 | """ 341 | Utility class that groups services together in one convenient structure. 342 | 343 | Args: 344 | service_details (obj:`tuple`): list of (str, str, srvType, func) tuples representing 345 | (unique_name, topic_name, service_type, callback) 346 | specifications for creating services 347 | 348 | Examples: 349 | Convert the incoming list of specifications into proper variables of this class. 350 | 351 | .. code-block:: python 352 | 353 | services = py_trees.utilities.Services( 354 | [ 355 | ('open_foo', '~/get_foo', foo_interfaces.srv.OpenFoo, open_foo_callback), 356 | ('open_foo', '/foo/open', foo_interfaces.srv.OpenFoo, self.open_foo_callback), 357 | ('get_foo_bar', '/foo/bar', foo_interfaces.srv.GetBar, self.foo.get_bar_callback), 358 | ] 359 | ) 360 | """ 361 | def __init__(self, node, service_details, introspection_topic_name="services"): 362 | # TODO: check for the correct setting of subscriber_details 363 | self.service_details_msg = [] 364 | for (name, service_name, service_type, callback) in service_details: 365 | self.__dict__[name] = node.create_service( 366 | srv_type=service_type, 367 | srv_name=service_name, 368 | callback=callback, 369 | qos_profile=rclpy.qos.qos_profile_services_default 370 | ) 371 | resolved_name = resolve_name(node, service_name) 372 | service_type = service_type.__class__.__module__.split('.')[0] + "/" + service_type.__class__.__name__ 373 | self.service_details_msg.append( 374 | py_trees_msgs.ServiceDetails( 375 | service_name=resolved_name, 376 | service_type=service_type, 377 | ) 378 | ) 379 | 380 | self.introspection_service = node.create_service( 381 | py_trees_srvs.IntrospectServices, 382 | "~/introspection/" + introspection_topic_name, 383 | self.introspection_callback 384 | ) 385 | 386 | def introspection_callback(self, unused_request, response): 387 | response.subscriber_details = self.subscriber_details_msg 388 | return response 389 | -------------------------------------------------------------------------------- /py_trees_ros/version.py: -------------------------------------------------------------------------------- 1 | # 2 | # License: BSD 3 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 4 | # 5 | ############################################################################## 6 | # Documentation 7 | ############################################################################## 8 | 9 | """ 10 | Version number provided separately here so there is easy access for the module, 11 | setup.py and sphinx. 12 | """ 13 | 14 | ############################################################################## 15 | # Version 16 | ############################################################################## 17 | 18 | __version__ = '2.2.2' 19 | -------------------------------------------------------------------------------- /py_trees_ros/visitors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE 6 | # 7 | ############################################################################## 8 | # Documentation 9 | ############################################################################## 10 | 11 | """ 12 | ROS Visitors are entities that can be passed to a ROS tree implementation 13 | (e.g. :class:`~py_trees_ros.trees.BehaviourTree`) and used to either visit 14 | each and every behaviour in the tree, or visit behaviours as the tree is 15 | traversed in an executing tick. At each behaviour, the visitor 16 | runs its own method on the behaviour to do as it wishes - logging, introspecting). 17 | 18 | .. warning:: Visitors should not modify the behaviours they visit. 19 | 20 | .. seealso:: The base interface and core visitors in :mod:`py_trees.visitors` 21 | """ 22 | 23 | ############################################################################## 24 | # Imports 25 | ############################################################################## 26 | 27 | 28 | import py_trees.visitors 29 | import py_trees_ros_interfaces.msg as py_trees_msgs 30 | import rclpy 31 | import time 32 | 33 | from . import conversions 34 | 35 | ############################################################################## 36 | # Visitors 37 | ############################################################################## 38 | 39 | 40 | class SetupLogger(py_trees.visitors.VisitorBase): 41 | """ 42 | Use as a visitor to :meth:`py_trees_ros.trees.TreeManager.setup` 43 | to log the name and timings of each behaviours' setup 44 | to the ROS debug channel. 45 | 46 | Args: 47 | node: an rclpy node that will provide debug logger 48 | """ 49 | def __init__(self, node: rclpy.node.Node): 50 | super().__init__(full=True) 51 | self.node = node 52 | 53 | def initialise(self): 54 | """ 55 | Initialise the timestamping chain. 56 | """ 57 | self.start_time = time.monotonic() 58 | self.last_time = self.start_time 59 | 60 | def run(self, behaviour): 61 | current_time = time.monotonic() 62 | self.node.get_logger().debug( 63 | "'{}'.setup: {:.4f}s".format(behaviour.name, current_time - self.last_time) 64 | ) 65 | self.last_time = current_time 66 | 67 | def finalise(self): 68 | current_time = time.monotonic() 69 | self.node.get_logger().debug( 70 | "Total tree setup time: {:.4f}s".format(current_time - self.start_time) 71 | ) 72 | 73 | 74 | class TreeToMsgVisitor(py_trees.visitors.VisitorBase): 75 | """ 76 | Visits the entire tree and gathers all behaviours as 77 | messages for the tree logging publishers. 78 | 79 | Attributes: 80 | tree (:class:`py_trees_msgs.msg.BehaviourTree`): tree representation in message form 81 | """ 82 | def __init__(self): 83 | """ 84 | Well 85 | """ 86 | super(TreeToMsgVisitor, self).__init__() 87 | self.full = True # examine all nodes 88 | 89 | def initialise(self): 90 | """ 91 | Initialise and stamp a :class:`py_trees_msgs.msg.BehaviourTree` 92 | instance. 93 | """ 94 | self.tree = py_trees_msgs.BehaviourTree() 95 | # TODO: crystal api 96 | # self.tree.stamp = rclpy.clock.Clock.now().to_msg() 97 | 98 | def run(self, behaviour): 99 | """ 100 | Convert the behaviour into a message and append to the tree. 101 | 102 | Args: 103 | behaviour (:class:`~py_trees.behaviour.Behaviour`): behaviour to convert 104 | """ 105 | self.tree.behaviours.append(conversions.behaviour_to_msg(behaviour)) 106 | -------------------------------------------------------------------------------- /resources/py_trees_ros: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/splintered-reality/py_trees_ros/9326c9df5b603439583d2ad29b50dd3052614630/resources/py_trees_ros -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [pep8] 2 | max-line-length=299 3 | 4 | [aliases] 5 | test=pytest 6 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from setuptools import find_packages, setup 5 | 6 | package_name = 'py_trees_ros' 7 | 8 | setup( 9 | name=package_name, 10 | version='2.3.0', # also package.xml, doc/conf.py, py_trees_ros/version.py 11 | packages=find_packages( 12 | exclude=['doc*', 'tests*', 'graveyard*', 'scripts*'] 13 | ), 14 | data_files=[ 15 | ('share/' + package_name, ['package.xml']), 16 | ('share/ament_index/resource_index/packages', [ 17 | 'resources/py_trees_ros']), 18 | ], 19 | package_data={}, 20 | install_requires=[], 21 | extras_require={}, 22 | author='Daniel Stonier, Naveed Usmani, Michal Staniaszek', 23 | maintainer='Daniel Stonier , Sebastian Castro ', 24 | url='https://github.com/splintered-reality/py_trees_ros', 25 | keywords=['ROS', 'behaviour-trees'], 26 | zip_safe=True, 27 | classifiers=[ 28 | 'Intended Audience :: Developers', 29 | 'License :: OSI Approved :: BSD License', 30 | 'Programming Language :: Python', 31 | 'Topic :: Scientific/Engineering :: Artificial Intelligence', 32 | 'Topic :: Software Development :: Libraries' 33 | ], 34 | description=( 35 | "ROS extensions for py-trees, a pythonic implementation of " 36 | "behaviour trees." 37 | ), 38 | long_description=( 39 | "ROS extensions for py-trees, a pythonic implementation of " 40 | "behaviour trees. It includes ROS specific behaviours a tree" 41 | "manager with ROS communication handles for debugging and" 42 | "visualisation, logging and various tutorials." 43 | ), 44 | license='BSD', 45 | test_suite='tests', 46 | tests_require=[], # using vanilla py unit tests 47 | entry_points={ 48 | 'console_scripts': [ 49 | 'py-trees-blackboard-watcher = py_trees_ros.programs.blackboard_watcher:main', 50 | # 'py-trees-echo = py_trees_ros.programs.echo:main', 51 | # 'py-trees-multi-talker = py_trees_ros.programs.multi_talker:main', 52 | 'py-trees-tree-watcher = py_trees_ros.programs.tree_watcher:main', 53 | ], 54 | }, 55 | ) 56 | -------------------------------------------------------------------------------- /tests/README.md: -------------------------------------------------------------------------------- 1 | # Executing Tests 2 | 3 | ```bash 4 | # run all tests in the current directory 5 | $ pytest-3 6 | 7 | # run all tests with full stdout (-s / --capture=no) 8 | $ pytest-3 -s 9 | 10 | # run a test module 11 | $ pytest-3 -s test_action_client.py 12 | 13 | # run a single test 14 | $ pytest-3 -s test_action_clients.py::test_success 15 | 16 | # run using setuptools 17 | $ python3 setup.py test 18 | ``` -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | import importlib 2 | import unittest 3 | 4 | class ImportTest(unittest.TestCase): 5 | 6 | def test_import(self): 7 | """ 8 | This test serves to make the buildfarm happy in Python 3.12 and later. 9 | See https://github.com/colcon/colcon-core/issues/678 for more information. 10 | """ 11 | assert importlib.util.find_spec("py_trees_ros") 12 | -------------------------------------------------------------------------------- /tests/test_action_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | import py_trees_ros_interfaces.action as py_trees_actions # noqa 16 | import rclpy 17 | import rclpy.action 18 | import rclpy.executors 19 | import time 20 | 21 | ############################################################################## 22 | # Helpers 23 | ############################################################################## 24 | 25 | 26 | def assert_banner(): 27 | print(console.green + "----- Asserts -----" + console.reset) 28 | 29 | 30 | def assert_details(text, expected, result): 31 | print(console.green + text + 32 | "." * (40 - len(text)) + 33 | console.cyan + "{}".format(expected) + 34 | console.yellow + " [{}]".format(result) + 35 | console.reset) 36 | 37 | 38 | def create_action_client(from_blackboard=False, wait_for_server_timeout_sec=2.0): 39 | if from_blackboard: 40 | behaviour = py_trees_ros.action_clients.FromBlackboard( 41 | name="dock", 42 | action_type=py_trees_actions.Dock, 43 | action_name="dock", 44 | key="goal", 45 | generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed), 46 | wait_for_server_timeout_sec=wait_for_server_timeout_sec 47 | ) 48 | else: 49 | behaviour = py_trees_ros.action_clients.FromConstant( 50 | name="dock", 51 | action_type=py_trees_actions.Dock, 52 | action_name="dock", 53 | action_goal=py_trees_actions.Dock.Goal(dock=True), # noqa 54 | generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed), 55 | wait_for_server_timeout_sec=wait_for_server_timeout_sec 56 | ) 57 | return behaviour 58 | 59 | 60 | def print_unicode_tree(tree): 61 | print( 62 | "\n" + 63 | py_trees.display.unicode_tree( 64 | tree.root, 65 | visited=tree.snapshot_visitor.visited, 66 | previously_visited=tree.snapshot_visitor.previously_visited 67 | ) 68 | ) 69 | 70 | 71 | class RejectGoalServer(object): 72 | def __init__(self, node_name, action_name, action_type): 73 | self.node = rclpy.create_node(node_name) 74 | 75 | self.action_server = rclpy.action.ActionServer( 76 | node=self.node, 77 | action_type=action_type, 78 | action_name=action_name, 79 | callback_group=rclpy.callback_groups.ReentrantCallbackGroup(), # needed? 80 | execute_callback=lambda goal_handle: None, 81 | goal_callback=self.goal_callback 82 | ) 83 | 84 | def goal_callback(self, unused_goal_request): 85 | return rclpy.action.server.GoalResponse.REJECT 86 | 87 | def shutdown(self): 88 | self.action_server.destroy() 89 | self.node.destroy_node() 90 | 91 | 92 | class DockFailedServer(py_trees_ros.mock.dock.Dock): 93 | def __init__(self): 94 | super().__init__() 95 | 96 | def execute_goal_callback(self, goal_handle): 97 | result = self.action_type.Result() 98 | goal_handle.abort() 99 | return result 100 | 101 | 102 | def setup_module(module): 103 | console.banner("ROS Init") 104 | rclpy.init() 105 | 106 | 107 | def teardown_module(module): 108 | console.banner("ROS Shutdown") 109 | rclpy.shutdown() 110 | 111 | 112 | def timeout(): 113 | return 3.0 114 | 115 | 116 | def number_of_iterations(): 117 | return 100 118 | 119 | ############################################################################## 120 | # Tests 121 | ############################################################################## 122 | 123 | 124 | ######################################## 125 | # Success 126 | ######################################## 127 | 128 | def test_success(): 129 | console.banner("Client Success") 130 | 131 | server = py_trees_ros.mock.dock.Dock(duration=1.5) 132 | 133 | root = create_action_client() 134 | tree = py_trees_ros.trees.BehaviourTree( 135 | root=root, 136 | unicode_tree_debug=False 137 | ) 138 | tree.setup() 139 | 140 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 141 | executor.add_node(server.node) 142 | executor.add_node(tree.node) 143 | 144 | assert_banner() 145 | 146 | assert_details("root.status", "INVALID", root.status) 147 | assert(root.status == py_trees.common.Status.INVALID) 148 | 149 | tree.tick() 150 | 151 | assert_details("root.status", "RUNNING", root.status) 152 | assert(root.status == py_trees.common.Status.RUNNING) 153 | 154 | tree.tick_tock( 155 | period_ms=100, 156 | number_of_iterations=number_of_iterations() 157 | ) 158 | 159 | while tree.count < number_of_iterations() and root.status == py_trees.common.Status.RUNNING: 160 | executor.spin_once(timeout_sec=0.05) 161 | 162 | assert_details("root.status", "SUCCESS", root.status) 163 | assert(root.status == py_trees.common.Status.SUCCESS) 164 | 165 | tree.shutdown() 166 | server.shutdown() 167 | executor.shutdown() 168 | 169 | ######################################## 170 | # Priority Interrupt 171 | ######################################## 172 | 173 | 174 | def test_priority_interrupt(): 175 | console.banner("Priority Interrupt") 176 | 177 | server = py_trees_ros.mock.dock.Dock(duration=1.5) 178 | 179 | action_client = create_action_client() 180 | success_eventually = py_trees.behaviours.StatusQueue( 181 | name="Success Eventually", 182 | queue=[ 183 | py_trees.common.Status.FAILURE, 184 | py_trees.common.Status.FAILURE, 185 | py_trees.common.Status.FAILURE, 186 | py_trees.common.Status.FAILURE 187 | ], 188 | eventually=py_trees.common.Status.SUCCESS, 189 | ) 190 | root = py_trees.composites.Selector(name="Selector", memory=False) 191 | root.add_children([success_eventually, action_client]) 192 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 193 | tree.setup() 194 | 195 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 196 | executor.add_node(server.node) 197 | executor.add_node(tree.node) 198 | 199 | assert_banner() 200 | 201 | tree.tick() 202 | print_unicode_tree(tree) 203 | 204 | assert_details("action_client.status", "RUNNING", root.status) 205 | assert(action_client.status == py_trees.common.Status.RUNNING) 206 | 207 | tree.tick_tock( 208 | period_ms=100, 209 | number_of_iterations=number_of_iterations() 210 | ) 211 | 212 | while tree.count < number_of_iterations() and "cancelled" not in action_client.feedback_message: 213 | executor.spin_once(timeout_sec=0.1) 214 | print_unicode_tree(tree) 215 | assert_details("action_client.status", "INVALID", action_client.status) 216 | assert(action_client.status == py_trees.common.Status.INVALID) 217 | 218 | # hack to make sure action client is in a state that lets it 219 | # shut down without segfaulting 220 | if action_client.get_result_future is not None: 221 | while not action_client.get_result_future.done(): 222 | executor.spin_once(timeout_sec=0.1) 223 | 224 | tree.shutdown() 225 | server.shutdown() 226 | executor.shutdown() 227 | 228 | ######################################## 229 | # Rejection 230 | ######################################## 231 | 232 | 233 | def test_rejection(): 234 | console.banner("Client Rejection") 235 | 236 | server = RejectGoalServer( 237 | node_name="reject", 238 | action_name="dock", 239 | action_type=py_trees_actions.Dock, 240 | ) 241 | 242 | root = create_action_client() 243 | tree = py_trees_ros.trees.BehaviourTree(root=root) 244 | 245 | # ROS Setup 246 | tree.setup() 247 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 248 | executor.add_node(server.node) 249 | executor.add_node(tree.node) 250 | tree.tick() 251 | tree.tick_tock(period_ms=100, number_of_iterations=number_of_iterations()) 252 | 253 | # ROS Spin 254 | while tree.count < number_of_iterations() and root.status == py_trees.common.Status.RUNNING: 255 | executor.spin_once(timeout_sec=0.05) 256 | 257 | print("") 258 | assert_banner() 259 | assert_details("root.status", "FAILURE", root.status) 260 | assert(root.status == py_trees.common.Status.FAILURE) 261 | 262 | tree.shutdown() 263 | server.shutdown() 264 | executor.shutdown() 265 | 266 | ######################################## 267 | # Aborted 268 | ######################################## 269 | 270 | 271 | def test_aborted(): 272 | console.banner("Server Aborted") 273 | 274 | server = DockFailedServer() 275 | 276 | root = create_action_client() 277 | tree = py_trees_ros.trees.BehaviourTree(root=root) 278 | 279 | # ROS Setup 280 | tree.setup() 281 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 282 | executor.add_node(server.node) 283 | executor.add_node(tree.node) 284 | tree.tick() 285 | tree.tick_tock(period_ms=100, number_of_iterations=number_of_iterations()) 286 | 287 | # ROS Spin 288 | while tree.count < number_of_iterations() and root.status == py_trees.common.Status.RUNNING: 289 | executor.spin_once(timeout_sec=0.05) 290 | 291 | print("") 292 | assert_banner() 293 | assert_details("root.status", "FAILURE", root.status) 294 | assert(root.status == py_trees.common.Status.FAILURE) 295 | 296 | tree.shutdown() 297 | server.shutdown() 298 | executor.shutdown() 299 | 300 | ######################################## 301 | # From Blackboard 302 | ######################################## 303 | 304 | 305 | def test_from_blackboard(): 306 | console.banner("From Blackboard") 307 | 308 | server = py_trees_ros.mock.dock.Dock(duration=1.5) 309 | 310 | root = create_action_client(from_blackboard=True) 311 | tree = py_trees_ros.trees.BehaviourTree(root=root) 312 | 313 | # ROS Setup 314 | tree.setup() 315 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 316 | executor.add_node(server.node) 317 | executor.add_node(tree.node) 318 | 319 | print("") 320 | assert_banner() 321 | 322 | tree.tick() 323 | 324 | # Nothing on blackboard yet 325 | assert_details("No goal on blackboard - root.status", "FAILURE", root.status) 326 | assert(root.status == py_trees.common.Status.FAILURE) 327 | 328 | py_trees.blackboard.Blackboard.set( 329 | variable_name="/goal", 330 | value=py_trees_actions.Dock.Goal(dock=True) # noqa 331 | ) 332 | 333 | tree.tick() 334 | 335 | # Goal exists on blackboard 336 | assert_details("Goal exists - root.status", "RUNNING", root.status) 337 | assert(root.status == py_trees.common.Status.RUNNING) 338 | 339 | tree.shutdown() 340 | server.shutdown() 341 | executor.shutdown() 342 | 343 | ######################################## 344 | # Timeouts 345 | ######################################## 346 | 347 | 348 | def test_timeouts(): 349 | console.banner("Timeouts") 350 | 351 | timeout = 0.1 352 | root = create_action_client(from_blackboard=True, wait_for_server_timeout_sec=timeout) 353 | tree = py_trees_ros.trees.BehaviourTree(root=root) 354 | 355 | start_time = time.monotonic() 356 | try: 357 | tree.setup() 358 | except py_trees_ros.exceptions.TimedOutError: 359 | duration = time.monotonic() - start_time 360 | assert_details("Tree Setup Timeout", "delta+{}".format(timeout), duration) 361 | assert(duration < 10*timeout) 362 | 363 | tree.shutdown() 364 | -------------------------------------------------------------------------------- /tests/test_blackboard.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import threading 13 | 14 | import py_trees 15 | import py_trees.console as console 16 | import py_trees_ros 17 | import rclpy 18 | 19 | ############################################################################## 20 | # Helpers 21 | ############################################################################## 22 | 23 | 24 | def assert_banner(): 25 | print(console.green + "----- Asserts -----" + console.reset) 26 | 27 | 28 | def assert_details(text, expected, result): 29 | print(console.green + text + 30 | "." * (40 - len(text)) + 31 | console.cyan + "{}".format(expected) + 32 | console.yellow + " [{}]".format(result) + 33 | console.reset) 34 | 35 | 36 | def setup_module(module): 37 | console.banner("ROS Init") 38 | rclpy.init() 39 | 40 | 41 | def teardown_module(module): 42 | console.banner("ROS Shutdown") 43 | rclpy.shutdown() 44 | 45 | ############################################################################## 46 | # Tests 47 | ############################################################################## 48 | 49 | 50 | def test_blackboard_pickle(): 51 | console.banner("Test Pickle Failure") 52 | node = rclpy.create_node("pickle") 53 | print("Set 'foo' to a thread lock object") 54 | py_trees.blackboard.Blackboard.set("foo", threading.Lock()) 55 | print("Create Sub Blackboard") 56 | sub_blackboard = py_trees_ros.blackboard.SubBlackboard(node=node) 57 | print("Update with warning - will raise exceptions if pickle errors are not caught") 58 | sub_blackboard.update({"foo"}) 59 | assert(True) 60 | print("Update Quietly") 61 | sub_blackboard.update({"foo"}) 62 | assert(True) 63 | -------------------------------------------------------------------------------- /tests/test_expand_topic_name.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | import rclpy 16 | import rclpy.executors 17 | import tf2_ros 18 | 19 | import geometry_msgs.msg as geometry_msgs 20 | 21 | ############################################################################## 22 | # Helpers 23 | ############################################################################## 24 | 25 | 26 | def assert_banner(): 27 | print(console.green + "----- Asserts -----" + console.reset) 28 | 29 | 30 | def assert_details(text, expected, result): 31 | print(console.green + text + 32 | "." * (40 - len(text)) + 33 | console.cyan + "{}".format(expected) + 34 | console.yellow + " [{}]".format(result) + 35 | console.reset) 36 | 37 | 38 | def setup_module(module): 39 | console.banner("ROS Init") 40 | rclpy.init() 41 | 42 | 43 | def teardown_module(module): 44 | console.banner("ROS Shutdown") 45 | rclpy.shutdown() 46 | 47 | 48 | def number_of_iterations(): 49 | return 1000 50 | 51 | 52 | def source_frame(): 53 | return "noggin" 54 | 55 | 56 | def target_frame(): 57 | return "noodle" 58 | 59 | ############################################################################## 60 | # Tests 61 | ############################################################################## 62 | 63 | 64 | def test_snapshot_stream_topic_name(): 65 | console.banner("Snapshot Stream Topic Names") 66 | pairs = { 67 | None: '/tree/snapshot_streams/_snapshots_0', 68 | "foo": "/tree/snapshot_streams/foo", 69 | "~/foo": "/tree/foo", 70 | "/bar/foo": "/bar/foo" 71 | } 72 | default_node_name = "tree" 73 | node = rclpy.create_node(node_name=default_node_name) 74 | for name, expected in pairs.items(): 75 | expanded = py_trees_ros.trees.SnapshotStream.expand_topic_name(node, name) 76 | assert_details(str(name), expected, expanded) 77 | assert(expanded == expected) 78 | node.destroy_node() 79 | -------------------------------------------------------------------------------- /tests/test_publishers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | import pytest 16 | import rclpy 17 | import rclpy.executors 18 | import std_msgs.msg as std_msgs 19 | import time 20 | 21 | ############################################################################## 22 | # Helpers 23 | ############################################################################## 24 | 25 | 26 | def assert_banner(): 27 | print(console.green + "----- Asserts -----" + console.reset) 28 | 29 | 30 | def assert_details(text, expected, result): 31 | print(console.green + text + 32 | "." * (40 - len(text)) + 33 | console.cyan + "{}".format(expected) + 34 | console.yellow + " [{}]".format(result) + 35 | console.reset) 36 | 37 | 38 | def setup_module(module): 39 | console.banner("ROS Init") 40 | rclpy.init() 41 | 42 | 43 | def teardown_module(module): 44 | console.banner("ROS Shutdown") 45 | rclpy.shutdown() 46 | 47 | 48 | def topic_name(): 49 | return "/empty" 50 | 51 | 52 | def blackboard_key(): 53 | return "/empty" 54 | 55 | 56 | def timeout(): 57 | return 0.3 58 | 59 | 60 | def number_of_iterations(): 61 | return 3 62 | 63 | 64 | def qos_profile(): 65 | return py_trees_ros.utilities.qos_profile_unlatched() 66 | 67 | 68 | class Subscriber(object): 69 | def __init__(self, node_name, topic_name, topic_type, qos_profile): 70 | 71 | self.node = rclpy.create_node(node_name) 72 | self.subscriber = self.node.create_subscription( 73 | msg_type=topic_type, 74 | topic=topic_name, 75 | callback=self.callback, 76 | qos_profile=qos_profile 77 | ) 78 | self.count = 0 79 | 80 | def callback(self, msg): 81 | self.count += 1 82 | 83 | def shutdown(self): 84 | self.subscriber.destroy() 85 | self.node.destroy_node() 86 | 87 | 88 | def create_all_the_things(): 89 | subscriber = Subscriber( 90 | node_name="catcher_of_nothing", 91 | topic_name=topic_name(), 92 | topic_type=std_msgs.Empty, 93 | qos_profile=qos_profile() 94 | ) 95 | root = py_trees_ros.publishers.FromBlackboard( 96 | name="From Blackboard", 97 | topic_name=topic_name(), 98 | topic_type=std_msgs.Empty, 99 | qos_profile=qos_profile(), 100 | blackboard_variable=blackboard_key() 101 | ) 102 | return subscriber, root 103 | 104 | ############################################################################## 105 | # Tests 106 | ############################################################################## 107 | 108 | 109 | def test_publish_with_existing_data(): 110 | console.banner("Publish Existing Data") 111 | 112 | py_trees.blackboard.Blackboard.set( 113 | variable_name=blackboard_key(), 114 | value=std_msgs.Empty() 115 | ) 116 | 117 | subscriber, root = create_all_the_things() 118 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 119 | tree.setup() 120 | executor = rclpy.executors.SingleThreadedExecutor() 121 | executor.add_node(subscriber.node) 122 | executor.add_node(tree.node) 123 | 124 | assert_banner() 125 | 126 | start_time = time.monotonic() 127 | while ((time.monotonic() - start_time) < timeout()) and subscriber.count == 0: 128 | tree.tick() 129 | executor.spin_once(timeout_sec=0.05) 130 | 131 | assert_details("root.status", "SUCCESS", root.status) 132 | assert(root.status == py_trees.common.Status.SUCCESS) 133 | 134 | py_trees.blackboard.Blackboard.clear() 135 | tree.shutdown() 136 | subscriber.shutdown() 137 | executor.shutdown() 138 | 139 | 140 | def test_fail_with_no_data(): 141 | console.banner("Fail with No Data") 142 | 143 | subscriber, root = create_all_the_things() 144 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 145 | tree.setup() 146 | executor = rclpy.executors.SingleThreadedExecutor() 147 | executor.add_node(subscriber.node) 148 | executor.add_node(tree.node) 149 | assert_banner() 150 | 151 | tree.tick() 152 | executor.spin_once(timeout_sec=0.05) 153 | 154 | assert_details("root.status", "FAILURE", root.status) 155 | assert(root.status == py_trees.common.Status.FAILURE) 156 | 157 | tree.shutdown() 158 | subscriber.shutdown() 159 | executor.shutdown() 160 | 161 | 162 | def test_exception_with_wrong_data(): 163 | console.banner("Exception with Wrong Data") 164 | 165 | py_trees.blackboard.Blackboard.set( 166 | variable_name=blackboard_key(), 167 | value=5.0 168 | ) 169 | 170 | subscriber, root = create_all_the_things() 171 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 172 | tree.setup() 173 | executor = rclpy.executors.SingleThreadedExecutor() 174 | executor.add_node(subscriber.node) 175 | executor.add_node(tree.node) 176 | assert_banner() 177 | 178 | with pytest.raises(TypeError) as unused_e_info: # e_info survives outside this block 179 | tree.tick() 180 | 181 | tree.shutdown() 182 | subscriber.shutdown() 183 | executor.shutdown() 184 | -------------------------------------------------------------------------------- /tests/test_service_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | from std_srvs.srv import SetBool, Trigger 16 | import rclpy 17 | import rclpy.action 18 | import rclpy.executors 19 | import time 20 | 21 | ############################################################################## 22 | # Helpers 23 | ############################################################################## 24 | 25 | 26 | def assert_banner(): 27 | print(console.green + "----- Asserts -----" + console.reset) 28 | 29 | 30 | def assert_details(text, expected, result): 31 | print(console.green + text + 32 | "." * (40 - len(text)) + 33 | console.cyan + "{}".format(expected) + 34 | console.yellow + " [{}]".format(result) + 35 | console.reset) 36 | 37 | 38 | def create_service_client(from_blackboard=False, key_response=None, wait_for_server_timeout_sec=2.0): 39 | if from_blackboard: 40 | behaviour = py_trees_ros.service_clients.FromBlackboard( 41 | name="set_bool_client", 42 | service_type=SetBool, 43 | service_name="set_bool", 44 | key_request="request", 45 | key_response=key_response, 46 | wait_for_server_timeout_sec=wait_for_server_timeout_sec 47 | ) 48 | else: 49 | behaviour = py_trees_ros.service_clients.FromConstant( 50 | name="set_bool_client", 51 | service_type=SetBool, 52 | service_name="set_bool", 53 | service_request=SetBool.Request(data=True), 54 | key_response=key_response, 55 | wait_for_server_timeout_sec=wait_for_server_timeout_sec 56 | ) 57 | return behaviour 58 | 59 | 60 | def print_unicode_tree(tree): 61 | print( 62 | "\n" + 63 | py_trees.display.unicode_tree( 64 | tree.root, 65 | visited=tree.snapshot_visitor.visited, 66 | previously_visited=tree.snapshot_visitor.previously_visited 67 | ) 68 | ) 69 | 70 | 71 | def setup_module(module): 72 | console.banner("ROS Init") 73 | rclpy.init() 74 | 75 | 76 | def teardown_module(module): 77 | console.banner("ROS Shutdown") 78 | rclpy.shutdown() 79 | 80 | 81 | def timeout(): 82 | return 3.0 83 | 84 | 85 | def number_of_iterations(): 86 | return 100 87 | 88 | ############################################################################## 89 | # Tests 90 | ############################################################################## 91 | 92 | 93 | ######################################## 94 | # Success, From Constant 95 | ######################################## 96 | 97 | def test_success(): 98 | console.banner("Client Success") 99 | 100 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=1.0) 101 | 102 | root = create_service_client() 103 | tree = py_trees_ros.trees.BehaviourTree( 104 | root=root, 105 | unicode_tree_debug=False 106 | ) 107 | tree.setup() 108 | 109 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 110 | executor.add_node(server.node) 111 | executor.add_node(tree.node) 112 | 113 | assert_banner() 114 | 115 | assert_details("root.status", "INVALID", root.status) 116 | assert(root.status == py_trees.common.Status.INVALID) 117 | 118 | tree.tick() 119 | 120 | assert_details("root.status", "RUNNING", root.status) 121 | assert(root.status == py_trees.common.Status.RUNNING) 122 | 123 | tree.tick_tock( 124 | period_ms=100, 125 | number_of_iterations=number_of_iterations() 126 | ) 127 | 128 | while tree.count < number_of_iterations() and root.status == py_trees.common.Status.RUNNING: 129 | executor.spin_once(timeout_sec=0.05) 130 | 131 | assert_details("root.status", "SUCCESS", root.status) 132 | assert(root.status == py_trees.common.Status.SUCCESS) 133 | 134 | tree.shutdown() 135 | server.shutdown() 136 | executor.shutdown() 137 | 138 | ######################################## 139 | # Success, From Constant, Write Response to Blackboard 140 | ######################################## 141 | 142 | def test_success_write_to_blackboard(): 143 | console.banner("Client Success, Write to Blackboard") 144 | 145 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=1.0) 146 | 147 | root = create_service_client(key_response="response") 148 | tree = py_trees_ros.trees.BehaviourTree( 149 | root=root, 150 | unicode_tree_debug=False 151 | ) 152 | tree.setup() 153 | 154 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 155 | executor.add_node(server.node) 156 | executor.add_node(tree.node) 157 | 158 | assert_banner() 159 | 160 | tree.tick() 161 | 162 | tree.tick_tock( 163 | period_ms=100, 164 | number_of_iterations=number_of_iterations() 165 | ) 166 | 167 | while tree.count < number_of_iterations() and root.status == py_trees.common.Status.RUNNING: 168 | executor.spin_once(timeout_sec=0.05) 169 | 170 | assert_details("root.status", "SUCCESS", root.status) 171 | assert(root.status == py_trees.common.Status.SUCCESS) 172 | 173 | blackboard = py_trees.blackboard.Client() 174 | blackboard.register_key(key="response", access=py_trees.common.Access.READ) 175 | assert_details("blackboard.response.success", True, blackboard.response.success) 176 | assert(blackboard.response.success == True) 177 | assert_details("blackboard.response.message", py_trees_ros.mock.set_bool.SetBoolServer.SUCCESS_MESSAGE, blackboard.response.message) 178 | assert(blackboard.response.message == py_trees_ros.mock.set_bool.SetBoolServer.SUCCESS_MESSAGE) 179 | 180 | tree.shutdown() 181 | server.shutdown() 182 | executor.shutdown() 183 | 184 | ######################################## 185 | # Success, From Blackboard 186 | ######################################## 187 | 188 | def test_success_from_blackboard(): 189 | console.banner("Client Success, From Blackboard") 190 | 191 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=1.0) 192 | 193 | root = create_service_client(from_blackboard=True) 194 | tree = py_trees_ros.trees.BehaviourTree( 195 | root=root, 196 | unicode_tree_debug=False 197 | ) 198 | tree.setup() 199 | 200 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 201 | executor.add_node(server.node) 202 | executor.add_node(tree.node) 203 | 204 | assert_banner() 205 | 206 | tree.tick() 207 | 208 | # Nothing on blackboard yet 209 | assert_details("No request on blackboard - root.status", "FAILURE", root.status) 210 | assert(root.status == py_trees.common.Status.FAILURE) 211 | 212 | py_trees.blackboard.Blackboard.set( 213 | variable_name="/request", 214 | value=SetBool.Request(data=True) # noqa 215 | ) 216 | 217 | tree.tick() 218 | 219 | # Request exists on blackboard 220 | assert_details("Request exists - root.status", "RUNNING", root.status) 221 | assert(root.status == py_trees.common.Status.RUNNING) 222 | 223 | tree.shutdown() 224 | server.shutdown() 225 | executor.shutdown() 226 | 227 | ######################################## 228 | # Success, Long Service Call 229 | ######################################## 230 | 231 | def test_success_long_service_call(): 232 | console.banner("Client Success, Long Service Call") 233 | 234 | service_time = 5.0 # secs 235 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=service_time) 236 | 237 | root = create_service_client() 238 | tree = py_trees_ros.trees.BehaviourTree( 239 | root=root, 240 | unicode_tree_debug=False 241 | ) 242 | tree.setup() 243 | 244 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 245 | executor.add_node(server.node) 246 | executor.add_node(tree.node) 247 | 248 | assert_banner() 249 | 250 | assert_details("root.status", "INVALID", root.status) 251 | assert(root.status == py_trees.common.Status.INVALID) 252 | 253 | start_time = time.monotonic() 254 | while time.monotonic() - start_time < service_time: 255 | tree.tick() 256 | assert_details("Remaining time %f secs - root.status" % (service_time - (time.monotonic() - start_time)), "RUNNING", root.status) 257 | assert(root.status == py_trees.common.Status.RUNNING) 258 | time.sleep(0.2) 259 | executor.spin_once(timeout_sec=0.05) 260 | 261 | time.sleep(0.2) 262 | tree.tick() 263 | 264 | assert_details("root.status", "SUCCESS", root.status) 265 | assert(root.status == py_trees.common.Status.SUCCESS) 266 | 267 | tree.shutdown() 268 | server.shutdown() 269 | executor.shutdown() 270 | 271 | ######################################## 272 | # Failure, Server does not exist 273 | ######################################## 274 | 275 | 276 | def test_failure_server_does_not_exist(): 277 | console.banner("Client Failure, Server does not exist") 278 | 279 | timeout = 0.1 280 | root = create_service_client(from_blackboard=True, wait_for_server_timeout_sec=timeout) 281 | tree = py_trees_ros.trees.BehaviourTree(root=root) 282 | 283 | start_time = time.monotonic() 284 | try: 285 | tree.setup() 286 | except py_trees_ros.exceptions.TimedOutError: 287 | duration = time.monotonic() - start_time 288 | assert_details("Tree Setup Timeout", "delta+{}".format(timeout), duration) 289 | assert(duration < 10*timeout) 290 | 291 | tree.shutdown() 292 | 293 | ######################################## 294 | # Failure, Request of wrong type 295 | ######################################## 296 | 297 | def test_failure_from_blackboard(): 298 | console.banner("Client Failure, Request of wrong type") 299 | 300 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=1.0) 301 | 302 | root = create_service_client(from_blackboard=True) 303 | tree = py_trees_ros.trees.BehaviourTree( 304 | root=root, 305 | unicode_tree_debug=False 306 | ) 307 | tree.setup() 308 | 309 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 310 | executor.add_node(server.node) 311 | executor.add_node(tree.node) 312 | 313 | assert_banner() 314 | 315 | py_trees.blackboard.Blackboard.set( 316 | variable_name="/request", 317 | value=Trigger.Request() # noqa 318 | ) 319 | 320 | tree.tick() 321 | 322 | # Request of wrong type 323 | assert_details("Request of wrong type - root.status", "FAILURE", root.status) 324 | assert(root.status == py_trees.common.Status.FAILURE) 325 | 326 | tree.shutdown() 327 | server.shutdown() 328 | executor.shutdown() 329 | 330 | ######################################## 331 | # Priority Interrupt 332 | ######################################## 333 | 334 | def test_priority_interrupt(): 335 | console.banner("Priority Interrupt") 336 | 337 | sleep_time = 3.0 338 | server = py_trees_ros.mock.set_bool.SetBoolServer(sleep_time=sleep_time) 339 | 340 | service_client = create_service_client() 341 | success_eventually = py_trees.behaviours.StatusQueue( 342 | name="Success Eventually", 343 | queue=[ 344 | py_trees.common.Status.FAILURE, 345 | ], 346 | eventually=py_trees.common.Status.SUCCESS, 347 | ) 348 | root = py_trees.composites.Selector(name="Selector", memory=False) 349 | root.add_children([success_eventually, service_client]) 350 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 351 | tree.setup() 352 | 353 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 354 | executor.add_node(server.node) 355 | executor.add_node(tree.node) 356 | 357 | assert_banner() 358 | 359 | tree.tick() 360 | 361 | assert_details("Pre-failure: service_client.status", "RUNNING", service_client.status) 362 | assert(service_client.status == py_trees.common.Status.RUNNING) 363 | 364 | tree.tick() 365 | 366 | assert_details("Post-failure: service_client.status", "INVALID", service_client.status) 367 | assert(service_client.status == py_trees.common.Status.INVALID) 368 | 369 | # Sleep longer than sleep time to ensure nothing happens after the server callback ends 370 | time.sleep(sleep_time*1.5) 371 | 372 | tree.shutdown() 373 | server.shutdown() 374 | executor.shutdown() -------------------------------------------------------------------------------- /tests/test_subscribers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | import rclpy 16 | import rclpy.executors 17 | import std_msgs.msg as std_msgs 18 | 19 | ############################################################################## 20 | # Helpers 21 | ############################################################################## 22 | 23 | 24 | def assert_banner(): 25 | print(console.green + "----- Asserts -----" + console.reset) 26 | 27 | 28 | def assert_details(text, expected, result): 29 | print(console.green + text + 30 | "." * (40 - len(text)) + 31 | console.cyan + "{}".format(expected) + 32 | console.yellow + " [{}]".format(result) + 33 | console.reset) 34 | 35 | 36 | def setup_module(module): 37 | console.banner("ROS Init") 38 | rclpy.init() 39 | 40 | 41 | def teardown_module(module): 42 | console.banner("ROS Shutdown") 43 | rclpy.shutdown() 44 | 45 | 46 | def timeout(): 47 | return 0.3 48 | 49 | 50 | def number_of_iterations(): 51 | return 100 52 | 53 | 54 | def qos_profile(): 55 | return py_trees_ros.utilities.qos_profile_unlatched() 56 | 57 | 58 | class EmptyPublisher(object): 59 | def __init__(self, node_name, qos_profile): 60 | 61 | self.node = rclpy.create_node(node_name) 62 | self._publisher = self.node.create_publisher( 63 | msg_type=std_msgs.Empty, 64 | topic="~/empty", 65 | qos_profile=qos_profile 66 | ) 67 | self.timer = self.node.create_timer( 68 | timer_period_sec=0.1, 69 | callback=self.publish 70 | ) 71 | 72 | def publish(self): 73 | self._publisher.publish(std_msgs.Empty()) 74 | 75 | def shutdown(self): 76 | self._publisher.destroy() 77 | self.timer.cancel() 78 | self.node.destroy_timer(self.timer) 79 | self.node.destroy_node() 80 | 81 | ############################################################################## 82 | # Tests 83 | ############################################################################## 84 | 85 | 86 | def test_wait_for_data(): 87 | console.banner("Wait for Data") 88 | 89 | publisher = EmptyPublisher(node_name="wait_for_data", qos_profile=qos_profile()) 90 | root = py_trees_ros.subscribers.WaitForData( 91 | name="WaitForData", 92 | topic_name="/wait_for_data/empty", 93 | topic_type=std_msgs.Empty, 94 | qos_profile=qos_profile() 95 | ) 96 | tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False) 97 | tree.setup() 98 | 99 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 100 | executor.add_node(publisher.node) 101 | executor.add_node(tree.node) 102 | 103 | assert_banner() 104 | 105 | tree.tick_tock( 106 | period_ms=100, 107 | number_of_iterations=number_of_iterations() 108 | ) 109 | 110 | while tree.count < number_of_iterations() and root.status != py_trees.common.Status.SUCCESS: 111 | executor.spin_once(timeout_sec=0.05) 112 | 113 | assert_details("root.status", "SUCCESS", root.status) 114 | assert(root.status == py_trees.common.Status.SUCCESS) 115 | 116 | tree.shutdown() 117 | publisher.shutdown() 118 | executor.shutdown() 119 | -------------------------------------------------------------------------------- /tests/test_transforms.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # License: BSD 5 | # https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE 6 | # 7 | 8 | ############################################################################## 9 | # Imports 10 | ############################################################################## 11 | 12 | import py_trees 13 | import py_trees.console as console 14 | import py_trees_ros 15 | import rclpy 16 | import rclpy.executors 17 | import tf2_ros 18 | 19 | import geometry_msgs.msg as geometry_msgs 20 | 21 | ############################################################################## 22 | # Helpers 23 | ############################################################################## 24 | 25 | 26 | def assert_banner(): 27 | print(console.green + "----- Asserts -----" + console.reset) 28 | 29 | 30 | def assert_details(text, expected, result): 31 | print(console.green + text + 32 | "." * (40 - len(text)) + 33 | console.cyan + "{}".format(expected) + 34 | console.yellow + " [{}]".format(result) + 35 | console.reset) 36 | 37 | 38 | def setup_module(module): 39 | console.banner("ROS Init") 40 | rclpy.init() 41 | 42 | 43 | def teardown_module(module): 44 | console.banner("ROS Shutdown") 45 | rclpy.shutdown() 46 | 47 | 48 | def number_of_iterations(): 49 | return 1000 50 | 51 | 52 | def source_frame(): 53 | return "noggin" 54 | 55 | 56 | def target_frame(): 57 | return "noodle" 58 | 59 | 60 | def qos_profile(): 61 | return py_trees_ros.utilities.qos_profile_unlatched() 62 | 63 | 64 | class Broadcaster(object): 65 | def __init__(self): 66 | self.node = rclpy.create_node("broadcast") 67 | self.broadcaster = tf2_ros.TransformBroadcaster( 68 | node=self.node, 69 | qos=qos_profile()) 70 | self.x = 0.3 71 | self.timer = self.node.create_timer( 72 | timer_period_sec=0.1, 73 | callback=self.send_transform 74 | ) 75 | 76 | def send_transform(self): 77 | t = geometry_msgs.TransformStamped() 78 | t.header.stamp = rclpy.clock.Clock().now().to_msg() 79 | t.header.frame_id = source_frame() 80 | t.child_frame_id = target_frame() 81 | t.transform.translation.x = self.x 82 | t.transform.translation.y = 0.0 83 | t.transform.translation.z = 0.0 84 | t.transform.rotation.x = 0.0 85 | t.transform.rotation.y = 0.0 86 | t.transform.rotation.z = 0.0 87 | t.transform.rotation.w = 1.0 88 | print(console.green + "Broadcast: {}".format(self.x) + console.reset) 89 | self.broadcaster.sendTransform(t) 90 | self.x += 0.1 91 | 92 | def shutdown(self): 93 | self.timer.cancel() 94 | self.node.destroy_timer(self.timer) 95 | self.node.destroy_node() 96 | 97 | 98 | class create_behaviours(object): 99 | 100 | def __init__(self, static): 101 | self.static = static 102 | 103 | def __enter__(self): 104 | transform = geometry_msgs.Transform() 105 | transform.translation.x = 3.0 106 | transform.translation.y = 0.0 107 | transform.translation.z = 0.0 108 | transform.rotation.x = 0.0 109 | transform.rotation.y = 0.0 110 | transform.rotation.z = 0.0 111 | transform.rotation.w = 1.0 112 | py_trees.blackboard.Blackboard.set("transform", transform) 113 | self.from_blackboard = py_trees_ros.transforms.FromBlackboard( 114 | variable_name="transform", 115 | target_frame=target_frame(), 116 | source_frame=source_frame(), 117 | qos_profile=qos_profile(), 118 | static=self.static, 119 | name="From Blackboard" 120 | ) 121 | self.to_blackboard = py_trees_ros.transforms.ToBlackboard( 122 | variable_name="noggin_to_noodle", 123 | target_frame=target_frame(), 124 | source_frame=source_frame(), 125 | qos_profile=qos_profile(), 126 | name="To Blackboard" 127 | ) 128 | self.broadcaster_node = rclpy.create_node("broadcaster") 129 | self.listener_node = rclpy.create_node("listener") 130 | self.from_blackboard.setup(node=self.broadcaster_node) 131 | self.to_blackboard.setup(node=self.listener_node) 132 | self.executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 133 | self.executor.add_node(self.broadcaster_node) 134 | self.executor.add_node(self.listener_node) 135 | return (self.from_blackboard, self.to_blackboard, self.executor) 136 | 137 | def __exit__(self, unused_type, unused_value, unused_traceback): 138 | self.broadcaster_node.destroy_node() 139 | self.listener_node.destroy_node() 140 | self.executor.shutdown() 141 | 142 | 143 | ############################################################################## 144 | # Tests 145 | ############################################################################## 146 | 147 | 148 | def test_to_blackboard_success(): 149 | console.banner("To Blackboard Success") 150 | broadcaster = Broadcaster() 151 | to_blackboard = py_trees_ros.transforms.ToBlackboard( 152 | variable_name="noggin_to_noodle", 153 | target_frame=target_frame(), 154 | source_frame=source_frame(), 155 | qos_profile=qos_profile(), 156 | name="To Blackboard" 157 | ) 158 | node = rclpy.create_node("listener") 159 | to_blackboard.setup(node=node) 160 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 161 | executor.add_node(node) 162 | executor.add_node(broadcaster.node) 163 | 164 | count = 0 165 | while count < number_of_iterations() and to_blackboard.status != py_trees.common.Status.SUCCESS: 166 | print(console.green + "Listener::update().....{}".format(count) + console.reset) 167 | to_blackboard.tick_once() 168 | executor.spin_once(timeout_sec=0.05) 169 | count += 1 170 | 171 | assert_details("to_blackboard.status", "SUCCESS", to_blackboard.status) 172 | assert(to_blackboard.status == py_trees.common.Status.SUCCESS) 173 | 174 | broadcaster.shutdown() 175 | node.destroy_node() 176 | executor.shutdown() 177 | 178 | 179 | def test_to_blackboard_blocking(): 180 | console.banner("To Blackboard Blocking") 181 | to_blackboard = py_trees_ros.transforms.ToBlackboard( 182 | variable_name="noggin_to_noodle", 183 | target_frame=target_frame(), 184 | source_frame=source_frame(), 185 | qos_profile=qos_profile(), 186 | name="To Blackboard" 187 | ) 188 | node = rclpy.create_node("listener") 189 | to_blackboard.setup(node=node) 190 | executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) 191 | executor.add_node(node) 192 | 193 | to_blackboard.tick_once() 194 | executor.spin_once(timeout_sec=0.05) 195 | 196 | assert_details("to_blackboard.status", "RUNNING", to_blackboard.status) 197 | assert(to_blackboard.status == py_trees.common.Status.RUNNING) 198 | assert_details( 199 | "blackboard.noggin_to_noodle", 200 | None, 201 | to_blackboard.blackboard.noggin_to_noodle 202 | ) 203 | assert(to_blackboard.blackboard.noggin_to_noodle is None) 204 | 205 | node.destroy_node() 206 | executor.shutdown() 207 | 208 | 209 | def test_from_blackboard(): 210 | console.banner("From Blackboard") 211 | 212 | for static in {True, False}: 213 | with create_behaviours(static=True) as (from_blackboard, to_blackboard, executor): 214 | print(console.green + "---------------------------" + console.reset) 215 | print(console.green + "Static: {}".format(static) + console.reset) 216 | print(console.green + "---------------------------" + console.reset) 217 | count = 0 218 | while count < number_of_iterations() and to_blackboard.status != py_trees.common.Status.SUCCESS: 219 | print(console.green + "Tick.....{}".format(count) + console.reset) 220 | print(console.green + "Listener........{}".format(to_blackboard.feedback_message) + console.reset) 221 | print(console.green + "Broadcaster.....{}".format(from_blackboard.feedback_message) + console.reset) 222 | from_blackboard.tick_once() 223 | to_blackboard.tick_once() 224 | executor.spin_once(timeout_sec=0.05) 225 | count += 1 226 | 227 | assert_details("to_blackboard.status", "SUCCESS", to_blackboard.status) 228 | assert(to_blackboard.status == py_trees.common.Status.SUCCESS) 229 | --------------------------------------------------------------------------------