├── .clang-format ├── .configs ├── black.toml ├── mypy.ini └── ruff.toml ├── .devcontainer ├── Dockerfile └── devcontainer.json ├── .dockerignore ├── .github ├── CODEOWNERS ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── PULL_REQUEST_TEMPLATE.md └── workflows │ ├── ci.yml │ └── maintenance.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── bdai_ros2_wrappers ├── bdai_ros2_wrappers │ └── __init__.py ├── package.xml ├── resource │ └── bdai_ros2_wrappers ├── setup.cfg ├── setup.py └── test │ └── test_import.py └── synchros2 ├── README.md ├── examples ├── background_action_example.py ├── interruptible_talker_example.py ├── logs_to_ros_example.py ├── talker_example.py ├── talker_listener_example.py ├── tf_cli_example.py └── thread_affine_timer_example.py ├── package.xml ├── resource └── synchros2 ├── setup.cfg ├── setup.py ├── synchros2 ├── __init__.py ├── action.py ├── action_client.py ├── action_handle.py ├── callables.py ├── callback_groups.py ├── context.py ├── executors.py ├── feeds.py ├── filters.py ├── futures.py ├── graph.py ├── launch │ ├── __init__.py │ ├── actions.py │ ├── arguments.py │ ├── substitutions.py │ └── values.py ├── logging.py ├── node.py ├── process.py ├── publisher.py ├── py.typed ├── scope.py ├── service.py ├── service_handle.py ├── single_goal_action_server.py ├── single_goal_multiple_action_servers.py ├── static_transform_broadcaster.py ├── subscription.py ├── tf_listener_wrapper.py ├── time.py ├── type_hints.py └── utilities.py └── test ├── conftest.py ├── launch └── test_actions.py ├── test_action.py ├── test_action_client.py ├── test_action_handle.py ├── test_callables.py ├── test_context.py ├── test_executors.py ├── test_feeds.py ├── test_filters.py ├── test_futures.py ├── test_graph.py ├── test_integration.py ├── test_logging.py ├── test_node.py ├── test_process.py ├── test_publisher.py ├── test_service.py ├── test_service_handle.py ├── test_single_goal_action_server.py ├── test_single_goal_multiple_action_servers.py ├── test_static_transform_broadcaster.py ├── test_subscription.py ├── test_tf_listener_wrapper.py └── test_utilities.py /.clang-format: -------------------------------------------------------------------------------- 1 | # -*- yaml -*- 2 | 3 | # This file determines clang-format's style settings; for details, refer to 4 | # http://clang.llvm.org/docs/ClangFormatStyleOptions.html 5 | 6 | # See https://google.github.io/styleguide/cppguide.html for more info 7 | BasedOnStyle: Google 8 | 9 | Language: Cpp 10 | 11 | SortIncludes: false 12 | 13 | # https://clang.llvm.org/docs/ClangFormatStyleOptions.html#columnlimit 14 | # Maximum line with is 120 characters (default: 80) 15 | ColumnLimit : 120 16 | -------------------------------------------------------------------------------- /.configs/black.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 120 3 | target-version = ['py38'] 4 | include = '\.pyi?$' 5 | # `extend-exclude` is not honored when `black` is passed a file path explicitly, 6 | # as is typical when `black` is invoked via `pre-commit`. 7 | force-exclude = ''' 8 | /( 9 | docker/ros/.* 10 | )/ 11 | ''' 12 | 13 | preview = true 14 | -------------------------------------------------------------------------------- /.configs/mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | mypy_path = $MYPY_CONFIG_FILE_DIR/.. 3 | python_version = 3.8 4 | disallow_untyped_defs = True 5 | ignore_missing_imports = True 6 | explicit_package_bases = True 7 | check_untyped_defs = True 8 | strict_equality = True 9 | warn_unreachable = True 10 | warn_redundant_casts = True 11 | no_implicit_optional = True 12 | files = 13 | synchros2/synchros2, 14 | synchros2/test 15 | exclude = "^(docker|.*external|.*thirdparty|.*install|.*build|.*_experimental)/" 16 | -------------------------------------------------------------------------------- /.configs/ruff.toml: -------------------------------------------------------------------------------- 1 | # See https://docs.astral.sh/ruff/rules/ for ruff rules 2 | select = ["A", "B", "COM", "D101", "D102", "D103", "D2", "D3", "D402", "D417", "D419", "E", "ERA", "F", "I", "NPY", "RET", "RUF100", "SIM"] 3 | ignore = ["D203", "D213"] 4 | 5 | # Allow autofix for all enabled rules (when `--fix`) is provided. 6 | fixable = ["A", "B", "C", "D", "E", "F", "G", "I", "N", "Q", "S", "T", "W", "ANN", "ARG", "BLE", "COM", "DJ", "DTZ", "EM", "ERA", "EXE", "FBT", "ICN", "INP", "ISC", "NPY", "PD", "PGH", "PIE", "PL", "PT", "PTH", "PYI", "RET", "RSE", "RUF", "SIM", "SLF", "TCH", "TID", "TRY", "UP", "YTT"] 7 | unfixable = [] 8 | 9 | # Exclude a variety of commonly ignored directories. 10 | exclude = [ 11 | ".bzr", 12 | ".direnv", 13 | ".eggs", 14 | ".git", 15 | ".hg", 16 | ".hg", 17 | ".mypy_cache", 18 | ".nox", 19 | ".pants.d", 20 | ".pytype", 21 | ".ruff_cache", 22 | ".svn", 23 | ".tox", 24 | ".venv", 25 | "__pypackages__", 26 | "_build", 27 | "buck-out", 28 | "build", 29 | "dist", 30 | "node_modules", 31 | "venv", 32 | "docker/ros", 33 | ] 34 | 35 | # Same as Black. 36 | line-length = 120 37 | 38 | # Allow unused variables when underscore-prefixed. 39 | dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" 40 | 41 | # Assume Python 3.8. 42 | target-version = "py38" 43 | 44 | [per-file-ignores] 45 | "__init__.py" = ["F401"] 46 | 47 | # We don't require docstrings in tests 48 | "**/conftest.py" = ["D"] 49 | "**/test_*.py" = ["D"] 50 | "**/examples/*" = ["D"] 51 | 52 | [mccabe] 53 | # Unlike Flake8, default to a complexity level of 10. 54 | max-complexity = 10 55 | -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=humble 2 | FROM ros:${ROS_DISTRO}-ros-base 3 | 4 | ENV LANG=C.UTF-8 5 | ENV LC_ALL=C.UTF-8 6 | ENV ROS_DISTRO=${ROS_DISTRO} 7 | SHELL ["/bin/bash", "-c"] 8 | 9 | RUN DEBIAN_FRONTEND=noninteractive apt-get update -q && \ 10 | apt-get dist-upgrade -y --no-install-recommends && \ 11 | apt-get install -yq --no-install-recommends \ 12 | python3-pip \ 13 | python-is-python3 \ 14 | python3-argcomplete \ 15 | python3-colcon-common-extensions \ 16 | python3-colcon-mixin \ 17 | python3-pytest-cov \ 18 | python3-rosdep \ 19 | python3-vcstool && \ 20 | rm -rf /var/lib/apt/lists/* 21 | 22 | RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" 23 | 24 | RUN --mount=type=bind,source=.,target=/tmp/context \ 25 | apt-get update -q && rosdep update && \ 26 | rosdep install -y -i --from-paths /tmp/context && \ 27 | rm -rf /var/lib/apt/lists/* 28 | 29 | ENV RCUTILS_COLORIZED_OUTPUT=1 30 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "ros_utilities dev container", 3 | "build": { 4 | "dockerfile": "Dockerfile", 5 | "context": ".." 6 | } 7 | } 8 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | .git 2 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # Global owners 2 | * @tcappellari-bdai @khughes-bdai @mhidalgo-bdai 3 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve. 4 | title: '' 5 | labels: bug 6 | --- 7 | 8 | ## Bug description 9 | 10 | **Platform**: 11 | - OS [e.g. Ubuntu Jammy]: 12 | - ROS [e.g. Humble]: 13 | - `proto2ros` version [e.g. ag, commit sha]: 14 | 15 | ### How to reproduce 16 | 17 | 22 | 23 | **Expected behavior**: 24 | 25 | **Actual behavior**: 26 | 27 | ## Additional context 28 | 29 | 30 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for a new feature. 4 | title: '' 5 | labels: enhancement 6 | --- 7 | 8 | ## Feature description 9 | 10 | 11 | 12 | ## Additional considerations 13 | 14 | 15 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | ## Proposed changes 2 | 3 | 8 | 9 | ### Checklist 10 | 11 | 12 | 13 | - [ ] Lint and unit tests pass locally 14 | - [ ] I have added tests that prove my changes are effective 15 | - [ ] I have added necessary documentation to communicate the changes 16 | 17 | ### Additional comments 18 | 19 | 20 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: ros_utilities CI 2 | 3 | on: 4 | pull_request: 5 | push: 6 | branches: 7 | - main 8 | 9 | env: 10 | # Use docker.io for Docker Hub if empty 11 | REGISTRY: ghcr.io 12 | # github.repository as / 13 | IMAGE_NAME: bdaiinstitute/ros_utilities_jammy_humble 14 | 15 | defaults: 16 | run: 17 | shell: bash 18 | 19 | concurrency: 20 | group: ${{ github.repository }}-${{ github.workflow }}-${{ github.ref }}-${{ github.ref == 'refs/heads/main' && github.sha || ''}} 21 | cancel-in-progress: true 22 | 23 | jobs: 24 | lint: 25 | name: Lint ros_utilities packages 26 | runs-on: ubuntu-22.04 27 | steps: 28 | - name: Checkout repository 29 | uses: actions/checkout@v3 30 | - uses: actions/setup-python@v3 31 | - name: Lint sources 32 | uses: pre-commit/action@v3.0.0 33 | prepare_container: 34 | name: Prepare Humble container for tests 35 | runs-on: ubuntu-22.04 36 | needs: lint 37 | permissions: 38 | contents: read 39 | packages: write 40 | # This is used to complete the identity challenge 41 | # with sigstore/fulcio when running outside of PRs. 42 | id-token: write 43 | outputs: 44 | image: ${{ fromJSON(steps.meta.outputs.json).tags[0] }} 45 | steps: 46 | - name: Checkout repository 47 | uses: actions/checkout@v4 48 | 49 | - name: Setup Docker buildx # to workaround: https://github.com/docker/build-push-action/issues/461 50 | uses: docker/setup-buildx-action@79abd3f86f79a9d68a23c75a09a9a85889262adf 51 | 52 | - name: Log into registry ${{ env.REGISTRY }} 53 | uses: docker/login-action@v3 # https://github.com/docker/login-action 54 | with: 55 | registry: ${{ env.REGISTRY }} 56 | username: ${{ github.actor }} 57 | password: ${{ secrets.GITHUB_TOKEN }} 58 | 59 | - name: Extract metadata (tags, labels) for Docker 60 | uses: docker/metadata-action@v5 # https://github.com/docker/metadata-action 61 | with: 62 | images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} 63 | id: meta 64 | 65 | - name: Build and push Docker image (may be cached) 66 | uses: docker/build-push-action@v5 # https://github.com/docker/build-push-action 67 | with: 68 | context: . 69 | file: .devcontainer/Dockerfile 70 | push: true 71 | tags: ${{ steps.meta.outputs.tags }} 72 | labels: ${{ steps.meta.outputs.labels }} 73 | cache-from: type=gha 74 | cache-to: type=gha,mode=max 75 | build_and_test: 76 | name: Build and test ros_utilities packages 77 | runs-on: ubuntu-22.04 78 | needs: prepare_container 79 | container: 80 | image: ${{ needs.prepare_container.outputs.image }} 81 | steps: 82 | - name: Checkout repository 83 | uses: actions/checkout@v4 84 | 85 | - name: Build and run tests 86 | run: | 87 | source /opt/ros/humble/setup.bash 88 | colcon build --symlink-install 89 | source install/setup.bash 90 | colcon test --event-handlers console_direct+ 91 | colcon test-result --all --verbose 92 | -------------------------------------------------------------------------------- /.github/workflows/maintenance.yaml: -------------------------------------------------------------------------------- 1 | name: ros_utilities CI maintenance 2 | 3 | on: 4 | schedule: 5 | - cron: "0 0 * * 0" # once a week 6 | 7 | env: 8 | ORG_NAME: bdaiinstitute 9 | # github.repository as / 10 | IMAGE_NAME: ros_utilities_jammy_humble 11 | 12 | jobs: 13 | clean-ghcr: 14 | name: Prune old images from Github Container Registry 15 | runs-on: ubuntu-22.04 16 | steps: 17 | - name: Delete old pull request images 18 | uses: snok/container-retention-policy@v2 19 | with: 20 | image-names: ${{ env.IMAGE_NAME }} 21 | skip-tags: main 22 | cut-off: One week ago UTC 23 | org-name: ${{ env.ORG_NAME }} 24 | account-type: org 25 | token: ${{ secrets.GITHUB_TOKEN }} 26 | token-type: github-token 27 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # colcon 132 | build/ 133 | install/ 134 | log/ 135 | 136 | # catkin_make 137 | devel/ 138 | logs/ 139 | build/ 140 | bin/ 141 | lib/ 142 | 143 | # Editors 144 | .idea/ 145 | .vscode/ 146 | *~ 147 | *\#* 148 | .#* 149 | 150 | # ROS 151 | bag_files/ 152 | 153 | # Misc 154 | .DS_Store 155 | cyclonedds.xml 156 | 157 | # ROS bags 158 | *.db3 159 | *.bag 160 | 161 | # wandb 162 | wandb/ 163 | 164 | # Gitman-tracked external repos 165 | external 166 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | repos: 3 | - repo: https://github.com/charliermarsh/ruff-pre-commit 4 | # Ruff version. 5 | rev: 'v0.0.263' 6 | hooks: 7 | - id: ruff 8 | args: ['--fix', '--config', '.configs/ruff.toml'] 9 | - repo: https://github.com/psf/black 10 | rev: 23.3.0 11 | hooks: 12 | - id: black 13 | language_version: python3.10 14 | args: ['--config', '.configs/black.toml'] 15 | verbose: true 16 | - repo: https://github.com/pre-commit/pre-commit-hooks 17 | rev: v4.4.0 18 | hooks: 19 | - id: check-yaml 20 | args: ['--unsafe'] # details about the unsafe flag: 21 | # https://github.com/pre-commit/pre-commit-hooks#check-yaml 22 | # This is the solution proposed to prevent `check-yaml` from failing on custom tags: 23 | # https://github.com/pre-commit/pre-commit-hooks/issues/701 24 | - id: check-added-large-files 25 | args: ['--enforce-all', '--maxkb', '200'] 26 | - id: check-toml 27 | - id: end-of-file-fixer 28 | - id: check-merge-conflict 29 | - id: check-executables-have-shebangs 30 | - id: check-shebang-scripts-are-executable 31 | - repo: https://github.com/ssciwr/clang-format-hook.git 32 | rev: v14.0.0 33 | hooks: 34 | - id: clang-format 35 | types_or: [c++, c] 36 | - repo: https://github.com/cpplint/cpplint.git 37 | rev: 1.6.1 38 | hooks: 39 | - id: cpplint 40 | args: ['--quiet', '--filter=-whitespace/comments', '--linelength=120'] 41 | - repo: https://github.com/pre-commit/mirrors-mypy 42 | rev: v1.2.0 43 | hooks: 44 | - id: mypy 45 | args: ['--config-file', '.configs/mypy.ini'] 46 | pass_filenames: false 47 | additional_dependencies: 48 | - types-protobuf 49 | - types-requests 50 | - types-simplejson 51 | - types-ujson 52 | - types-PyYAML 53 | - types-toml 54 | - types-six 55 | - typing-extensions 56 | 57 | - repo: https://github.com/jumanjihouse/pre-commit-hooks 58 | rev: 3.0.0 59 | hooks: 60 | - id: forbid-binary 61 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | 2 | # Contributing to `ros_utilities` 3 | 4 | First off, thanks for taking the time to contribute! ❤️ 5 | 6 | All types of contributions are encouraged and valued. See the [Table of Contents](#table-of-contents) for different ways to help and details about how this project handles them. Please make sure to read the relevant section before making your contribution. It will make it a lot easier for us maintainers and smooth out the experience for all involved. The community looks forward to your contributions. 🎉 7 | 8 | > And if you like the project, but just don't have time to contribute, that's fine. There are other easy ways to support the project and show your appreciation, which we would also be very happy about: 9 | > - Star the project 10 | > - Tweet about it 11 | > - Refer this project in your project's readme 12 | > - Mention the project at local meetups and tell your friends/colleagues 13 | 14 | 15 | ## Table of Contents 16 | 17 | - [I Have a Question](#i-have-a-question) 18 | - [I Want To Contribute](#i-want-to-contribute) 19 | - [Reporting Bugs](#reporting-bugs) 20 | - [Requesting Features](#requesting-features) 21 | - [Your First Code Contribution](#your-first-code-contribution) 22 | - [Improving The Documentation](#improving-the-documentation) 23 | 24 | ## I Have a Question 25 | 26 | Before you ask a question, it is best to search for existing [issues](https://github.com/bdaiinstitute/ros_utilities/issues) that might help you. In case you have found a suitable issue and still need clarification, you can write your question in this issue. It is also advisable to search the internet for answers first. 27 | 28 | If you then still feel the need to ask a question and need clarification, we recommend the following: 29 | 30 | - Open an [issue](https://github.com/utilities/ros_utilities/issues/new). 31 | - Provide as much context as you can about what you're running into. 32 | - Provide project and platform versions, depending on what seems relevant. 33 | 34 | We will then take care of the issue as soon as possible. 35 | 36 | ## I Want To Contribute 37 | 38 | > [!IMPORTANT] 39 | > When contributing to this project, you must agree that you have authored 100% of the content, that you have the necessary rights to the content, and that the content you contribute may be provided under the project license. 40 | 41 | ### Reporting Bugs 42 | 43 | 44 | #### Before Submitting a Bug Report 45 | 46 | A good bug report shouldn't leave others needing to chase you up for more information. Therefore, we ask you to investigate carefully, collect information and describe the issue in detail in your report. Please complete the following steps in advance to help us fix any potential bug as fast as possible. 47 | 48 | - Make sure that you are using the latest version. 49 | - Determine if your bug is really a bug and not an error on your side. If you are looking for support, you might want to check [this section](#i-have-a-question)). 50 | - To see if other users have experienced (and potentially already solved) the same issue you are having, check if there is not already a bug report existing for your bug or error in the [bug tracker](https://github.com/bdaiinstitute/ros_utilities/issues?q=label%3Abug). 51 | - Collect information about the bug: 52 | - Stack trace (Traceback) 53 | - OS, ROS, Platform and Version (Windows, Linux, macOS, x86, ARM) 54 | - Package versions, package manager, depending on what seems relevant. 55 | - Possibly your input and the output 56 | - Can you reliably reproduce the issue? And can you also reproduce it with older versions? 57 | 58 | 59 | #### How Do I Submit a Good Bug Report? 60 | 61 | We use GitHub issues to track bugs and errors. If you run into an issue with the project: 62 | 63 | - Open an [issue](https://github.com/bdaiinstitute/ros_utilities/issues/new). 64 | - Explain the behavior you would expect and the actual behavior. 65 | - Please provide as much context as possible and describe the *reproduction steps* that someone else can follow to recreate the issue on their own. This usually includes your code. For good bug reports you should isolate the problem and create a reduced test case. 66 | - Provide the information you collected in the previous section. 67 | 68 | Once it's filed: 69 | 70 | - The project team will label the issue accordingly. 71 | - A team member will try to reproduce the issue with your provided steps. 72 | - If the team is able to reproduce the issue, it will be confirmed as a `bug` and the issue will be left to be [addressed by someone](#your-first-code-contribution). 73 | 74 | ### Requesting Features 75 | 76 | 77 | #### Before Submitting a Feature Request 78 | 79 | - Make sure that you are using the latest version. 80 | - Read the documentation carefully and ensure the functionality is indeed missing. 81 | - Perform a [search](https://github.com/bdaiinstitute/ros_utilities/issues) to see if the feature has already been requested. If it has, add a comment to the existing issue instead of opening a new one. 82 | - Find out whether your idea fits with the scope and aims of the project. It's up to you to make a strong case to convince the project's developers of the merits of this feature. Keep in mind that we want features that will be useful to the majority of our users and not just a small subset. If you're just targeting a minority of users, consider writing an add-on/plugin library. 83 | 84 | 85 | #### How Do I Submit a Good Feature Request? 86 | 87 | Feature requests are tracked as [GitHub issues](https://github.com/bdaiinstitute/ros_utilities/issues). 88 | 89 | - Use a **clear and descriptive title** for the issue to identify the suggestion. 90 | - Provide a **step-by-step description of the suggested enhancement** in as many details as possible. 91 | - **Describe the current behavior** and **explain which behavior you expected to see instead** and why. At this point you can also tell which alternatives do not work for you. 92 | - **Explain why this enhancement would be useful** to most `ros_utilities` users. You may also want to point out the other projects that solved it better and which could serve as inspiration. 93 | 94 | 95 | 96 | ### Your First Code Contribution 97 | 98 | - Install a suitable [ROS distribution](https://docs.ros.org/en/humble/Installation.html). 99 | 100 | - [Fork this repository](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/working-with-forks/fork-a-repo) on GitHub. 101 | 102 | - Clone your repository fork under a local ROS workspace: 103 | 104 | ```sh 105 | mkdir -p path/to/workspace/src 106 | cd path/to/workspace/src 107 | git clone https://github.com/user-name/ros_utilities.git 108 | cd - 109 | ``` 110 | 111 | - Install `pre-commit` hooks for it: 112 | 113 | ```sh 114 | cd path/to/workspace/src/ros_utilities 115 | pip install pre-commit 116 | pre-commit install 117 | cd - 118 | ``` 119 | 120 | - Make the intended changes with appropriate tests that validate it. 121 | - Push these changes and open a pull request against this repository. 122 | 123 | 124 | ## Attribution 125 | This guide is based on the **contributing-gen**. [Make your own](https://github.com/bttger/contributing-gen)! 126 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Boston Dynamics AI Institute 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # `ros_utilities` 2 | 3 | ![Python Support](https://img.shields.io/badge/python-3.8%20%7C%203.9%20%7C%203.10-blue) 4 | ![ROS Support](https://img.shields.io/badge/ROS-humble-blue) 5 | 6 | ## Overview 7 | 8 | `ros_utilities` enable a different, at times simpler approach to ROS 2 programming, particularly for those that come with a ROS 1 background. 9 | 10 | ## Packages 11 | 12 | This repository contains the following packages: 13 | 14 | | Package | Description | 15 | |-------------------------------------| -----------------------------------------------------------------------------------| 16 | | [`synchros2`](synchros2) | `rclpy` wrappers to ease idiomatic, synchronous ROS 2 programming in Python. | 17 | 18 | **Note**: `proto2ros` packages for Protobuf / ROS 2 interoperability used to live in this repository but now live in https://github.com/bdaiinstitute/proto2ros. 19 | 20 | ## Next steps 21 | 22 | See [contribution guidelines](CONTRIBUTING.md)! 23 | -------------------------------------------------------------------------------- /bdai_ros2_wrappers/bdai_ros2_wrappers/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import importlib 4 | import pkgutil 5 | import sys 6 | import warnings 7 | 8 | warnings.simplefilter("default") 9 | warnings.warn( 10 | "bdai_ros2_wrappers has been renamed to synchros2. Please use the new name synchros2 instead", 11 | DeprecationWarning, 12 | stacklevel=2, 13 | ) 14 | 15 | 16 | def aliased_import(name, alias): 17 | """Import a module or a package using an alias for it. 18 | 19 | For packages, this function will recursively import all its subpackages and modules. 20 | """ 21 | sys.modules[alias] = module = importlib.import_module(name) 22 | if hasattr(module, "__path__"): 23 | for info in pkgutil.iter_modules(module.__path__): 24 | aliased_import(f"{name}.{info.name}", f"{alias}.{info.name}") 25 | 26 | 27 | aliased_import("synchros2", alias=__name__) 28 | -------------------------------------------------------------------------------- /bdai_ros2_wrappers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bdai_ros2_wrappers 5 | 1.0.0 6 | Former AI Institute's wrappers for ROS 2, an alias for synchros2 7 | The AI Institute 8 | MIT 9 | 10 | synchros2 11 | 12 | 13 | ament_python 14 | 15 | 16 | -------------------------------------------------------------------------------- /bdai_ros2_wrappers/resource/bdai_ros2_wrappers: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/ros_utilities/bbe365dfafef15e9a6379246f801aca4d90a932b/bdai_ros2_wrappers/resource/bdai_ros2_wrappers -------------------------------------------------------------------------------- /bdai_ros2_wrappers/setup.cfg: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute, Inc. All rights reserved. 2 | 3 | [develop] 4 | script_dir=$base/lib/bdai_ros2_wrappers 5 | [install] 6 | install_scripts=$base/lib/bdai_ros2_wrappers 7 | -------------------------------------------------------------------------------- /bdai_ros2_wrappers/setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from setuptools import setup 4 | 5 | package_name = "bdai_ros2_wrappers" 6 | 7 | setup( 8 | name=package_name, 9 | version="1.0.0", 10 | packages=[package_name], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | ], 15 | install_requires=["setuptools"], 16 | maintainer="The AI Institute", 17 | maintainer_email="opensource@theaiinstitute.com", 18 | description="The AI Institute's wrappers for ROS2", 19 | tests_require=["pytest"], 20 | zip_safe=True, 21 | license="MIT", 22 | ) 23 | -------------------------------------------------------------------------------- /bdai_ros2_wrappers/test/test_import.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import bdai_ros2_wrappers.scope 4 | import synchros2.scope 5 | 6 | 7 | def test_submodule_aliasing() -> None: 8 | assert id(bdai_ros2_wrappers.scope) == id(synchros2.scope) 9 | 10 | 11 | def test_global_aliasing() -> None: 12 | with bdai_ros2_wrappers.scope.top(global_=True) as top: 13 | assert synchros2.scope.current() is top 14 | -------------------------------------------------------------------------------- /synchros2/examples/background_action_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of a ROS 2 aware executable using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/background_action_example.py 9 | ``` 10 | 11 | And follow the instructions on screen. 12 | """ 13 | 14 | import sys 15 | from typing import Any 16 | 17 | from example_interfaces.action import Fibonacci 18 | from rclpy.action.client import ActionClient 19 | from rclpy.action.server import ActionServer, ServerGoalHandle 20 | 21 | import synchros2.process as ros_process 22 | from synchros2.node import Node 23 | 24 | 25 | class MinimalActionServer(Node): 26 | """A minimal ROS 2 node serving an example_interfaces.action.Fibonacci action.""" 27 | 28 | def __init__(self, node_name: str = "minimal_action_server", **kwargs: Any) -> None: 29 | super().__init__(node_name, **kwargs) 30 | self._action_server = ActionServer(self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback) 31 | 32 | def execute_callback(self, goal_handle: ServerGoalHandle) -> Fibonacci.Result: 33 | sequence = [0, 1] 34 | 35 | for i in range(1, goal_handle.request.order): 36 | sequence.append(sequence[i] + sequence[i - 1]) 37 | feedback = Fibonacci.Feedback() 38 | feedback.sequence = sequence 39 | goal_handle.publish_feedback(feedback) 40 | goal_handle.succeed() 41 | 42 | result = Fibonacci.Result() 43 | result.sequence = sequence 44 | return result 45 | 46 | 47 | @ros_process.main() 48 | def main() -> None: 49 | """Example entrypoint. 50 | 51 | It is configured almost as a prebaked ROS 2 aware process. That is, including a 52 | process-wide (ie. globally accessible) node, an autoscaling multi-threaded executor 53 | running in a background thread, automatic ``logging`` logs forwarding to the ROS 2 54 | logging system, and implicit namespacing for all ROS 2 nodes loaded in it based on 55 | the executable basename. A convenience for simple executables that need to use ROS 2 56 | without having to worry about ROS 2 (or its fallout, e.g. accidental deadblocking). 57 | 58 | When executed, an action server node is loaded, and the process-wide node is used to 59 | setup an action client. This action client is then used to implement a simple console 60 | application. 61 | """ 62 | main.load(MinimalActionServer) 63 | 64 | action_client = ActionClient(main.node, Fibonacci, "compute_fibonacci_sequence") 65 | assert action_client.wait_for_server(timeout_sec=5) 66 | 67 | while main.context.ok(): 68 | line = input("Please provide a Fibonacci sequence order (or press Enter to exit): ") 69 | if not line: 70 | break 71 | try: 72 | order = int(line) 73 | except ValueError: 74 | print(f" Hmm, '{line}' is not a number", file=sys.stderr) 75 | continue 76 | result = action_client.send_goal(Fibonacci.Goal(order=order)) 77 | print("Computed Fibonacci sequence: ", list(result.result.sequence)) 78 | print("Bye bye!") 79 | 80 | 81 | if __name__ == "__main__": 82 | main() 83 | -------------------------------------------------------------------------------- /synchros2/examples/interruptible_talker_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of an interruptible ROS 2 single node process using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/interruptible_talker_example.py chit chat 9 | ``` 10 | 11 | You can speed up publication with: 12 | 13 | ```sh 14 | python3 examples/interruptible_talker_example.py chit chat --ros-args -p rate:=5.0 # Hz 15 | ``` 16 | 17 | On Ctrl + C, the node will be interrupted. Another Ctrl + C will terminate the example 18 | and one last "goodbye" message will be published by the talker. 19 | """ 20 | 21 | import itertools 22 | import typing 23 | 24 | import std_msgs.msg 25 | from rclpy.executors import SingleThreadedExecutor 26 | 27 | import synchros2.process as ros_process 28 | from synchros2.executors import foreground 29 | from synchros2.node import Node 30 | 31 | 32 | class InterruptibleTalkerNode(Node): 33 | """A node that logs the same phrase periodically.""" 34 | 35 | def __init__(self, phrase: str, **kwargs: typing.Any) -> None: 36 | super().__init__("interruptible_talker", **kwargs) 37 | self.phrase = phrase 38 | self.counter = itertools.count(start=1) 39 | rate = self.declare_parameter("rate", 1.0).value 40 | self.pub = self.create_publisher(std_msgs.msg.String, "chat", 1) 41 | self.timer = self.create_timer(1 / rate, self.callback) 42 | 43 | def callback(self) -> None: 44 | message = f"{self.phrase} (#{next(self.counter)})" 45 | self.pub.publish(std_msgs.msg.String(data=message)) 46 | self.get_logger().info(message) 47 | 48 | def destroy_node(self) -> None: 49 | message = "Goodbye!" 50 | self.pub.publish(std_msgs.msg.String(data=message)) 51 | self.get_logger().info(message) 52 | super().destroy_node() 53 | 54 | 55 | @ros_process.main(prebaked=False, interruptible=True) 56 | def main(args: typing.Sequence[str]) -> None: 57 | """Example entrypoint, taking command-line arguments. 58 | 59 | It is configured as ROS 2 aware process. That is, no process-wide node, 60 | no background autoscaling multi-threaded executor, no log forwarding to the ROS 2 61 | logging system, and no implicit node namespacing. In other words, a run-off-the-mill 62 | executable that uses ROS 2. Well, almost. 63 | 64 | When executed, a single `InterruptibleTalkerNode` is instantiated and spinned in the 65 | foreground. This will continue indefinitely until the executable is interrupted 66 | (e.g. by a SIGINT on Ctrl + C). Another Ctrl + C will confirm the interruption. 67 | Any other key will resume execution. 68 | """ 69 | with foreground(SingleThreadedExecutor()) as main.executor: # noqa: SIM117 70 | with main.managed(InterruptibleTalkerNode, " ".join(args[1:])) as main.node: 71 | while True: 72 | try: 73 | main.executor.spin() 74 | except KeyboardInterrupt: 75 | input( 76 | "Press Ctrl + C again to confirm, or press any other key to continue", 77 | ) 78 | 79 | 80 | if __name__ == "__main__": 81 | main() 82 | -------------------------------------------------------------------------------- /synchros2/examples/logs_to_ros_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of `logging` logs forward to the ROS 2 logging system. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/logs_to_ros_example.py 9 | ``` 10 | """ 11 | 12 | import itertools 13 | import logging 14 | 15 | import rclpy 16 | 17 | from synchros2.logging import logs_to_ros 18 | 19 | 20 | def main() -> None: 21 | rclpy.init() 22 | try: 23 | node = rclpy.create_node("example_logger") 24 | counter = itertools.count(start=1) 25 | 26 | def callback() -> None: 27 | logging.info("Called back %d times", next(counter)) 28 | 29 | node.create_timer(1, callback) 30 | with logs_to_ros(node): 31 | rclpy.spin(node) 32 | except KeyboardInterrupt: 33 | pass 34 | finally: 35 | rclpy.try_shutdown() 36 | 37 | 38 | if __name__ == "__main__": 39 | root = logging.getLogger() 40 | root.setLevel(logging.INFO) 41 | main() 42 | -------------------------------------------------------------------------------- /synchros2/examples/talker_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of a ROS 2 single node process using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/talker_example.py chit chat 9 | ``` 10 | 11 | You can speed up publication with: 12 | 13 | ```sh 14 | python3 examples/talker_example.py chit chat --ros-args -p rate:=5.0 # Hz 15 | ``` 16 | """ 17 | 18 | import itertools 19 | import typing 20 | 21 | import std_msgs.msg 22 | 23 | import synchros2.process as ros_process 24 | from synchros2.node import Node 25 | 26 | 27 | class TalkerNode(Node): 28 | """A node that logs the same phrase periodically.""" 29 | 30 | def __init__(self, phrase: str, **kwargs: typing.Any) -> None: 31 | super().__init__("talker", **kwargs) 32 | self.phrase = phrase 33 | self.counter = itertools.count(start=1) 34 | rate = self.declare_parameter("rate", 1.0).value 35 | self.pub = self.create_publisher(std_msgs.msg.String, "chat", 1) 36 | self.timer = self.create_timer(1 / rate, self.callback) 37 | 38 | def callback(self) -> None: 39 | message = f"{self.phrase} (#{next(self.counter)})" 40 | self.pub.publish(std_msgs.msg.String(data=message)) 41 | self.get_logger().info(message) 42 | 43 | 44 | @ros_process.main(prebaked=False) 45 | def main(args: typing.Sequence[str]) -> None: 46 | """Example entrypoint, taking command-line arguments. 47 | 48 | It is configured as a regular ROS 2 aware process. That is, no process-wide node, 49 | no background autoscaling multi-threaded executor, no log forwarding to the ROS 2 50 | logging system, and no implicit node namespacing. In other words, a run-off-the-mill 51 | executable that uses ROS 2. 52 | 53 | When executed, a single `TalkerNode` is instantiated and spinned in foreground, on 54 | an implicitly instantiated autoscaling multi-threaded executor. This will continue 55 | indefinitely until the executable is interrupted (e.g. by a SIGINT on Ctrl + C). 56 | """ 57 | ros_process.spin(TalkerNode, " ".join(args[1:])) 58 | 59 | 60 | if __name__ == "__main__": 61 | main() 62 | -------------------------------------------------------------------------------- /synchros2/examples/talker_listener_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of a ROS 2 multi-node process using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/talker_listener_example.py 9 | ``` 10 | """ 11 | 12 | import itertools 13 | import typing 14 | 15 | import std_msgs.msg 16 | 17 | import synchros2.process as ros_process 18 | from synchros2.node import Node 19 | 20 | 21 | class TalkerNode(Node): 22 | """A node that publishes a phrase to a chat topic periodically.""" 23 | 24 | def __init__(self, **kwargs: typing.Any) -> None: 25 | super().__init__("talker", **kwargs) 26 | self.counter = itertools.count(start=1) 27 | self.pub = self.create_publisher(std_msgs.msg.String, "chat", 1) 28 | rate = self.declare_parameter("rate", 1.0).value 29 | self.timer = self.create_timer(1 / rate, self.callback) 30 | 31 | def callback(self) -> None: 32 | message = std_msgs.msg.String(data=f"Hi there, from {self.get_name()} (#{next(self.counter)})") 33 | self.pub.publish(message) 34 | 35 | 36 | class ListenerNode(Node): 37 | """A node that subscribes to a chat topic and echoes it.""" 38 | 39 | def __init__(self, **kwargs: typing.Any) -> None: 40 | super().__init__("listener", **kwargs) 41 | self.sub = self.create_subscription(std_msgs.msg.String, "chat", self.callback, 1) 42 | 43 | def callback(self, message: std_msgs.msg.String) -> None: 44 | self.get_logger().info(message.data) 45 | 46 | 47 | def graph(**kwargs: typing.Any) -> typing.Iterable[Node]: 48 | return [TalkerNode(**kwargs), ListenerNode(**kwargs)] 49 | 50 | 51 | @ros_process.main(prebaked=False, namespace=True) 52 | def main() -> None: 53 | """Example entrypoint. 54 | 55 | It is configured almost as a regular ROS 2 aware process. That is, no process-wide node, 56 | no background autoscaling multi-threaded executor, and no log forwarding to the ROS 2 57 | logging system, but any ROS 2 nodes loaded in it will be implicitly namespaced after 58 | the executable basename. A convenience to better organize executables bearing many 59 | ROS 2 nodes. 60 | 61 | When executed, a graph of ROS 2 nodes is spinned in foreground, on an implicitly instantiated 62 | autoscaling multi-threaded executor. This continues indefinitely until the executable is 63 | interrupted (e.g. by a SIGINT on Ctrl + C). 64 | """ 65 | ros_process.spin(graph) 66 | 67 | 68 | if __name__ == "__main__": 69 | main() 70 | -------------------------------------------------------------------------------- /synchros2/examples/tf_cli_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of a ROS 2 aware command using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/tf_cli_example.py 9 | ``` 10 | 11 | And follow the instructions on screen. 12 | """ 13 | 14 | import argparse 15 | import math 16 | import time 17 | from typing import Any, Iterable, List, Optional 18 | 19 | from geometry_msgs.msg import TransformStamped 20 | from rclpy.executors import SingleThreadedExecutor 21 | from tf2_ros.transform_broadcaster import TransformBroadcaster 22 | 23 | import synchros2.process as ros_process 24 | from synchros2.executors import background 25 | from synchros2.node import Node 26 | from synchros2.tf_listener_wrapper import TFListenerWrapper 27 | from synchros2.utilities import namespace_with 28 | 29 | 30 | class TFBroadcasterNode(Node): 31 | def __init__(self, tf_prefix: Optional[str] = None, **kwargs: Any) -> None: 32 | super().__init__("tf_broadcaster", **kwargs) 33 | self.tf_broadcaster = TransformBroadcaster(self) 34 | self.transforms: List[TransformStamped] = [] 35 | tree = [("world", "odom", 0.0, 1.0, math.degrees(90.0)), ("odom", "robot", 3.0, -0.5, math.degrees(-30.0))] 36 | for parent_frame, child_frame, x, y, theta in tree: 37 | transform = TransformStamped() 38 | transform.header.frame_id = namespace_with(tf_prefix, parent_frame) 39 | transform.child_frame_id = namespace_with(tf_prefix, child_frame) 40 | transform.transform.translation.x = x 41 | transform.transform.translation.y = y 42 | transform.transform.translation.z = 0.0 43 | transform.transform.rotation.x = 0.0 44 | transform.transform.rotation.y = 0.0 45 | transform.transform.rotation.z = math.sin(theta / 2) 46 | transform.transform.rotation.w = math.cos(theta / 2) 47 | self.transforms.append(transform) 48 | self.timer = self.create_timer(1.0, self.callback) 49 | self.callback() # do not wait 50 | 51 | def callback(self) -> None: 52 | stamp = self.get_clock().now().to_msg() 53 | for transform in self.transforms: 54 | transform.header.stamp = stamp 55 | self.tf_broadcaster.sendTransform(self.transforms) 56 | 57 | 58 | def cli() -> argparse.ArgumentParser: 59 | parser = argparse.ArgumentParser() 60 | parser.add_argument("-p", "--tf-prefix", type=str, default=None) 61 | parser.add_argument("-c", "--cache-time", type=float, default=None) 62 | parser.add_argument("-b", "--buffer-time", type=float, default=3.0) 63 | return parser 64 | 65 | 66 | def graph(args: argparse.Namespace, **kwargs: Any) -> Iterable[Node]: 67 | return [TFBroadcasterNode(args.tf_prefix, **kwargs), Node("tf_prompt", **kwargs)] 68 | 69 | 70 | def run(args: argparse.Namespace) -> None: 71 | print("Buffering transforms...") 72 | time.sleep(args.buffer_time) 73 | tf_listener = ros_process.tf_listener() 74 | assert tf_listener is not None 75 | print(tf_listener.buffer.all_frames_as_string()) 76 | while True: 77 | target_frame = input("Please provide target frame (or press Enter to exit): ") 78 | if not target_frame: 79 | break 80 | source_frame = input("Please provide source frame (or press Enter to reuse): ") 81 | if not source_frame: 82 | source_frame = target_frame 83 | target_frame = namespace_with(args.tf_prefix, target_frame) 84 | source_frame = namespace_with(args.tf_prefix, source_frame) 85 | transform = tf_listener.lookup_a_tform_b(target_frame, source_frame, wait_for_frames=True) 86 | t = transform.transform.translation 87 | q = transform.transform.rotation 88 | print(f"Transform {target_frame} -> {source_frame}") 89 | print(f" Translation: [x: {t.x}, y: {t.y}, z: {t.z}]") 90 | print(f" Rotation: [x: {q.x}, y: {q.y}, z: {q.z}, w: {q.w}]") 91 | print("---") 92 | print("Bye bye!") 93 | 94 | 95 | @ros_process.main(cli(), prebaked=False) 96 | def main(args: argparse.Namespace) -> None: 97 | """Example entrypoint. 98 | 99 | It is first configured as a regular ROS 2 aware process, but process-wide 100 | (i.e. globally accessible) executor and node are set immediately on start. 101 | Still, automatic ``logging`` logs forwarding to the ROS 2 logging system is 102 | disabled. Implicit namespacing is also disabled. This is suitable for more 103 | complex use cases, that require finer control over process configuration. 104 | 105 | When executed, a single threaded executor is pushed to the background and 106 | assigned as process-wide executor. A graph of ROS 2 nodes is instantiated 107 | and loaded, one of which is assigned as process-wide node. These are used 108 | indirectly by the actual console application. 109 | """ 110 | with background(SingleThreadedExecutor()) as main.executor, ros_process.managed(graph, args) as (_, main.node): 111 | main.tf_listener = TFListenerWrapper(main.node, cache_time_s=args.cache_time) 112 | run(args) 113 | 114 | 115 | if __name__ == "__main__": 116 | main() 117 | -------------------------------------------------------------------------------- /synchros2/examples/thread_affine_timer_example.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | """An example of a ROS 2 aware command using process-wide machinery. 4 | 5 | Run with: 6 | 7 | ```sh 8 | python3 examples/thread_affine_timer_example.py 9 | ``` 10 | """ 11 | 12 | import threading 13 | 14 | from rclpy.callback_groups import MutuallyExclusiveCallbackGroup 15 | 16 | import synchros2.process as ros_process 17 | from synchros2.executors import AutoScalingMultiThreadedExecutor, foreground 18 | from synchros2.node import Node 19 | 20 | 21 | @ros_process.main(prebaked=False, interruptible=True) 22 | def main() -> None: 23 | """Example entrypoint. 24 | 25 | It is first configured as a regular ROS 2 aware process, but process-wide 26 | (i.e. globally accessible) multi-threaded executor and node are set immediately 27 | on start. Still, automatic ``logging`` logs forwarding to the ROS 2 logging 28 | system is disabled. Implicit namespacing is also disabled. 29 | 30 | When executed, two timers are created on the process-wide node: the first timer 31 | is assigned to a user-defined callback group that is bound to a static thread pool 32 | with a single thread, whereas the second timer is bound to the default thread pool. 33 | Idle time for the default thread pool is set such that the second timer is likely 34 | to run each time on a new thread. Timer callbacks will print the ID of the thread 35 | they are run by. This will continue indefinitely until the executable is interrupted 36 | (e.g. by a SIGINT on Ctrl + C). 37 | """ 38 | with foreground(AutoScalingMultiThreadedExecutor(max_thread_idle_time=0.5)) as main.executor: # noqa: SIM117 39 | with main.managed(Node, "example_node") as main.node: 40 | thread_pool = main.executor.add_static_thread_pool(1) 41 | custom_callback_group = MutuallyExclusiveCallbackGroup() 42 | main.executor.bind(custom_callback_group, thread_pool) 43 | 44 | def thread_affine_timer_callback(): 45 | current_thread = threading.current_thread() 46 | print(f"Timer 1 is affine to {current_thread.name} ({current_thread.ident})") 47 | 48 | main.node.create_timer(1.0, thread_affine_timer_callback, custom_callback_group) 49 | 50 | def timer_callback(): 51 | current_thread = threading.current_thread() 52 | print(f"Timer 2 run by {current_thread.name} ({current_thread.ident})") 53 | 54 | main.node.create_timer(1.0, timer_callback) 55 | 56 | main.executor.spin() 57 | 58 | 59 | if __name__ == "__main__": 60 | main() 61 | -------------------------------------------------------------------------------- /synchros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | synchros2 5 | 1.0.0 6 | The AI Institute's wrappers for ROS2 7 | The AI Institute 8 | MIT 9 | 10 | action_msgs 11 | rclpy 12 | 13 | example_interfaces 14 | python3-typing-extensions 15 | python3-pytest 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /synchros2/resource/synchros2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/ros_utilities/bbe365dfafef15e9a6379246f801aca4d90a932b/synchros2/resource/synchros2 -------------------------------------------------------------------------------- /synchros2/setup.cfg: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute, Inc. All rights reserved. 2 | 3 | [develop] 4 | script_dir=$base/lib/synchros2 5 | [install] 6 | install_scripts=$base/lib/synchros2 7 | -------------------------------------------------------------------------------- /synchros2/setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from setuptools import find_packages, setup 4 | 5 | package_name = "synchros2" 6 | 7 | setup( 8 | name=package_name, 9 | version="1.0.0", 10 | packages=find_packages(exclude=["test"]), 11 | package_data={package_name: ["py.typed"]}, 12 | data_files=[ 13 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 14 | ("share/" + package_name, ["package.xml"]), 15 | ], 16 | install_requires=["setuptools"], 17 | maintainer="The AI Institute", 18 | maintainer_email="opensource@theaiinstitute.com", 19 | description="The AI Institute's wrappers for ROS2", 20 | tests_require=["pytest"], 21 | zip_safe=True, 22 | license="MIT", 23 | ) 24 | -------------------------------------------------------------------------------- /synchros2/synchros2/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | -------------------------------------------------------------------------------- /synchros2/synchros2/action_client.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from typing import Any, Callable, Optional, Type 3 | 4 | import rclpy.action 5 | from rclpy.node import Node 6 | 7 | import synchros2.scope as scope 8 | from synchros2.action_handle import ActionHandle 9 | 10 | 11 | class ActionClientWrapper(rclpy.action.ActionClient): 12 | """A wrapper for ros2's ActionClient for extra functionality""" 13 | 14 | def __init__( 15 | self, 16 | action_type: Type, 17 | action_name: str, 18 | node: Optional[Node] = None, 19 | wait_for_server: bool = True, 20 | ) -> None: 21 | """Constructor 22 | 23 | Args: 24 | action_type (Type): The type of the action 25 | action_name (str): The name of the action (for logging purposes) 26 | node (Optional[Node]): optional node for action client, defaults to the current process node 27 | wait_for_server (bool): Whether to wait for the server 28 | """ 29 | if node is None: 30 | node = scope.ensure_node() 31 | self._node = node 32 | super().__init__(self._node, action_type, action_name) 33 | if wait_for_server: 34 | self._node.get_logger().info(f"Waiting for action server for {action_name}") 35 | self.wait_for_server() 36 | self._node.get_logger().info("Found server") 37 | 38 | def send_goal_and_wait( 39 | self, 40 | action_name: str, 41 | goal: Optional[Any], 42 | timeout_sec: Optional[float] = None, 43 | ) -> Optional[Any]: 44 | """Sends an action goal and waits for the result 45 | 46 | Args: 47 | action_name (str): A representative name of the action for logging 48 | goal (Any): The Action Goal sent to the action server 49 | timeout_sec (Optional[float]): A timeout for waiting on a response/result 50 | from the action server. Setting to None creates no timeout 51 | 52 | Returns: 53 | Optional[Any]: 54 | """ 55 | if goal is None: 56 | self._node.get_logger.warn("Cannot send NULL ActionGoal") 57 | return None 58 | self._node.get_logger().info(f"Sending Action [{action_name}]") 59 | 60 | canceled = False 61 | 62 | def _on_cancel_succeeded() -> None: 63 | nonlocal canceled 64 | canceled = True 65 | 66 | failed = False 67 | 68 | def _on_failure() -> None: 69 | nonlocal failed 70 | failed = True 71 | 72 | handle = self.send_goal_async_handle(action_name=action_name, goal=goal, on_failure_callback=_on_failure) 73 | handle.set_on_cancel_success_callback(_on_cancel_succeeded) 74 | if not handle.wait_for_result(timeout_sec=timeout_sec): 75 | # If the action didn't fail and wasn't canceled then it timed out and should be canceled 76 | if not failed and not canceled: 77 | handle.cancel() 78 | self._node.get_logger().error(f"Action [{action_name}] timed out") 79 | return None 80 | 81 | return handle.result 82 | 83 | def send_goal_async_handle( 84 | self, 85 | action_name: str, 86 | goal: Any, 87 | *, 88 | result_callback: Optional[Callable[[Any], None]] = None, 89 | feedback_callback: Optional[Callable[[Any], None]] = None, 90 | on_failure_callback: Optional[Callable[[], None]] = None, 91 | ) -> ActionHandle: 92 | """Sends an action goal asynchronously and create an `ActionHandle` 93 | 94 | Args: 95 | action_name (str): A representative name of the action for logging 96 | goal (Action.Goal): The Action Goal sent to the action server 97 | result_callback (Optional[Callable[[Action.Result], None]]): A callback to process/handle 98 | the resulting feedback from executing the command 99 | feedback_callback (Optional[Callable[[Action.Feedback], None]]): A callback to process/handle 100 | the feedback received during the execution of the command 101 | on_failure_callback (Optional[Callable[[None], None]]): A callback to process/handle when 102 | the action fails for various reasons 103 | 104 | Returns: 105 | ActionHandle: An object to manage the asynchronous lifecycle of the action request 106 | """ 107 | handle = ActionHandle( 108 | action_name=action_name, 109 | logger=self._node.get_logger(), 110 | context=self._node.context, 111 | ) 112 | if result_callback is not None: 113 | handle.set_result_callback(result_callback) 114 | 115 | if on_failure_callback is not None: 116 | handle.set_on_failure_callback(on_failure_callback) 117 | 118 | if feedback_callback is None: 119 | send_goal_future = self.send_goal_async(goal) 120 | else: 121 | handle.set_feedback_callback(feedback_callback) 122 | send_goal_future = self.send_goal_async(goal, feedback_callback=handle.get_feedback_callback) 123 | handle.set_send_goal_future(send_goal_future) 124 | 125 | return handle 126 | -------------------------------------------------------------------------------- /synchros2/synchros2/action_handle.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from threading import Event 3 | from typing import Any, Callable, Optional 4 | 5 | from action_msgs.msg import GoalStatus 6 | from rclpy.action.client import ClientGoalHandle 7 | from rclpy.context import Context 8 | from rclpy.impl.rcutils_logger import RcutilsLogger 9 | from rclpy.task import Future 10 | from rclpy.utilities import get_default_context 11 | 12 | 13 | class ActionHandle: 14 | """Handle for the lifecycle of an action. 15 | 16 | Handles the two stage process that is getting a result after sending an ActionGoal to an ActionServer as well 17 | as holding the various callbacks for sending an ActionGoal (cancel, failure, feedback, result) 18 | """ 19 | 20 | def __init__(self, action_name: str, logger: Optional[RcutilsLogger] = None, context: Optional[Context] = None): 21 | """Constructor 22 | 23 | Args: 24 | action_name (str): The name of the action (for logging purposes) 25 | logger (Optional[RcutilsLogger]): An optional logger to use. If none is provided, one is created 26 | context (Optional[Context]): An optional ros context 27 | """ 28 | if context is None: 29 | context = get_default_context() 30 | self._action_name = action_name 31 | self._send_goal_future: Optional[Future] = None 32 | self._goal_handle: Optional[ClientGoalHandle] = None 33 | self._wait_for_acceptance_event: Event = Event() 34 | context.on_shutdown(self._wait_for_acceptance_event.set) 35 | self._get_result_future: Optional[Future] = None 36 | self._feedback_callback: Optional[Callable[[Any], None]] = None 37 | self._wait_for_result_event: Event = Event() 38 | context.on_shutdown(self._wait_for_result_event.set) 39 | self._result_callback: Optional[Callable[[Any], None]] = None 40 | self._on_failure_callback: Optional[Callable] = None 41 | self._cancel_future: Optional[Future] = None 42 | self._on_cancel_success_callback: Optional[Callable] = None 43 | self._on_cancel_failure_callback: Optional[Callable] = None 44 | self._exception: Optional[Exception] = None 45 | self._result: Optional[Any] = None 46 | if logger is None: 47 | self._logger = RcutilsLogger(name=f"{action_name} Handle") 48 | else: 49 | self._logger = logger 50 | 51 | @property 52 | def result(self) -> Optional[Any]: 53 | """Returns the result of the action if it has been received from the ActionServer, None otherwise""" 54 | if self._exception is not None: 55 | raise self._exception 56 | if self._result is None: 57 | return None 58 | return self._result.result 59 | 60 | def wait_for_result(self, timeout_sec: Optional[float] = None) -> bool: 61 | """Waits until a result is received or times out 62 | 63 | A result is received through succeed, abort, or cancel being called on the server. 64 | 65 | Args: 66 | timeout_sec (Optional[float]): A timeout in seconds. No timeout is used if None 67 | 68 | Returns: 69 | True if successful; False if the timeout was triggered or the action was rejected, cancelled, or abort 70 | """ 71 | return self._wait_for_result_event.wait(timeout=timeout_sec) and ( 72 | self._result is not None and self._result.status == GoalStatus.STATUS_SUCCEEDED 73 | ) 74 | 75 | def wait_for_acceptance(self, timeout_sec: Optional[float] = None) -> bool: 76 | """Waits until goal is accepted or timeout. 77 | 78 | Args: 79 | timeout_sec (Optional[float]): A timeout in seconds. No timeout is used if None 80 | 81 | Returns: 82 | True if successful; False if the timeout was triggered or the goal was rejected 83 | """ 84 | return self._wait_for_acceptance_event.wait(timeout=timeout_sec) and ( 85 | self._goal_handle is not None and self._goal_handle.accepted 86 | ) 87 | 88 | def set_send_goal_future(self, send_goal_future: Future) -> None: 89 | """Sets the future received from sending the `Action.Goal`""" 90 | self._send_goal_future = send_goal_future 91 | self._send_goal_future.add_done_callback(self._goal_response_callback) 92 | 93 | def set_feedback_callback(self, feedback_callback: Callable[[Any], None]) -> None: 94 | """Sets the callback used to process feedback received while an Action is being executed""" 95 | self._feedback_callback = feedback_callback 96 | 97 | def set_result_callback(self, result_callback: Callable[[Any], None]) -> None: 98 | """Sets the callback for processing the result from executing an Action""" 99 | self._result_callback = result_callback 100 | 101 | def set_on_failure_callback(self, on_failure_callback: Callable) -> None: 102 | """Set the callback to execute upon failure""" 103 | self._on_failure_callback = on_failure_callback 104 | 105 | def set_on_cancel_success_callback(self, on_cancel_success_callback: Callable) -> None: 106 | """Set the callback to execute upon successfully canceling the action""" 107 | self._on_cancel_success_callback = on_cancel_success_callback 108 | 109 | def set_on_cancel_failure_callback(self, on_cancel_failure_callback: Callable) -> None: 110 | """Set the callback to execute upon failing to cancel the action""" 111 | self._on_cancel_failure_callback = on_cancel_failure_callback 112 | 113 | def _goal_response_callback(self, future: Future) -> None: 114 | """Callback that handles receiving a response from the ActionServer 115 | 116 | Note: This does not handle receiving a result directly 117 | """ 118 | if future.result() is None: 119 | self._logger.error(f"Action request failed: {future.exception():!r}") 120 | self._failure() 121 | self._wait_for_acceptance_event.set() 122 | self._wait_for_result_event.set() 123 | return 124 | self._goal_handle = future.result() 125 | if not self._goal_handle.accepted: 126 | self._logger.error("Action rejected") 127 | self._failure() 128 | self._wait_for_acceptance_event.set() 129 | self._wait_for_result_event.set() 130 | return 131 | 132 | self._logger.info("Action accepted") 133 | 134 | self._get_result_future = self._goal_handle.get_result_async() 135 | self._get_result_future.add_done_callback(self._get_result_callback) 136 | self._wait_for_acceptance_event.set() 137 | 138 | def get_feedback_callback(self, feedback: Any) -> None: 139 | """Public feedback callback that can be given to the ActionClient 140 | 141 | Currently just passes the feedback to the user provided callback 142 | """ 143 | if self._feedback_callback is not None: 144 | self._feedback_callback(feedback) 145 | 146 | def _get_result_callback(self, future: Future) -> None: 147 | """Callback for pulling the resulting feedback out of the future and passing it to a user provided callback""" 148 | self._exception = future.exception() 149 | if self._exception is None: 150 | self._result = future.result() 151 | assert self._result is not None 152 | if self._result.status == GoalStatus.STATUS_SUCCEEDED: 153 | self._logger.info("Finished successfully") 154 | if self._result_callback is not None: 155 | self._result_callback(self._result.result) 156 | elif self._result.status == GoalStatus.STATUS_ABORTED: 157 | self._logger.info("Aborted") 158 | self._failure() 159 | elif self._result.status == GoalStatus.STATUS_CANCELED: 160 | self._logger.info("Canceled") 161 | if self._on_cancel_success_callback is not None: 162 | self._on_cancel_success_callback() 163 | self._wait_for_result_event.set() 164 | 165 | def cancel(self) -> None: 166 | """Send the desire to cancel a command""" 167 | self._logger.info("Canceling Action") 168 | 169 | if self._goal_handle is not None: 170 | self._cancel_future = self._goal_handle.cancel_goal_async() 171 | self._cancel_future.add_done_callback(self._cancel_response_callback) 172 | 173 | def _cancel_response_callback(self, future: Future) -> None: 174 | """Callback for handling the response to attempting to cancel a command""" 175 | cancel_response = future.result() 176 | if len(cancel_response.goals_canceling) == 0: 177 | self._logger.info("Cancel request rejected") 178 | if self._on_cancel_failure_callback is not None: 179 | self._on_cancel_failure_callback() 180 | else: 181 | self._logger.info("Cancel request accepted") 182 | 183 | def _failure(self) -> None: 184 | """Triggers the internal failure callback if it exists""" 185 | if self._on_failure_callback is not None: 186 | self._on_failure_callback() 187 | -------------------------------------------------------------------------------- /synchros2/synchros2/callback_groups.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import threading 3 | import typing 4 | import weakref 5 | 6 | import rclpy.callback_groups 7 | import rclpy.executors 8 | 9 | 10 | class NonReentrantCallbackGroup(rclpy.callback_groups.CallbackGroup): 11 | """A callback group to prevent concurrent execution of the same callback while allowing it for different callbacks. 12 | 13 | Note this behavior sits in between that offered by 14 | rclpy.callback_groups.MutuallyExclusiveCallbackGroup and 15 | rclpy.callback_groups.ReentrantCallbackGroup, as the former forbids 16 | concurrent execution and the latter allows it including multiple 17 | invocations of the same callback (e.g. a subscription handling multiple 18 | messages concurrently). 19 | 20 | See rclpy.callback_groups.CallbackGroup documentation for further reference. 21 | """ 22 | 23 | def __init__(self) -> None: 24 | """Constructor""" 25 | super().__init__() 26 | self._active_entities: typing.Set[rclpy.executors.WaitableEntityType] = set() 27 | self._lock = threading.Lock() 28 | 29 | def can_execute(self, entity: rclpy.executors.WaitableEntityType) -> bool: 30 | """Determine if a callback for an entity can be executed. 31 | 32 | Args: 33 | entity: A subscription, timer, client, service, or waitable instance. 34 | 35 | Returns: 36 | `True` if entity callback can be executed, `False` otherwise. 37 | """ 38 | with self._lock: 39 | assert weakref.ref(entity) in self.entities 40 | return entity not in self._active_entities 41 | 42 | def beginning_execution(self, entity: rclpy.executors.WaitableEntityType) -> bool: 43 | """Get permission for the callback from the group to begin executing an entity. 44 | 45 | If this returns `True` then `CallbackGroup.ending_execution` must be called after 46 | the callback has been executed. 47 | 48 | Arg: 49 | entity:A subscription, timer, client, service, or waitable instance. 50 | 51 | Returns: 52 | `True` if the callback can be executed, `False` otherwise. 53 | """ 54 | with self._lock: 55 | assert weakref.ref(entity) in self.entities 56 | if entity not in self._active_entities: 57 | self._active_entities.add(entity) 58 | return True 59 | return False 60 | 61 | def ending_execution(self, entity: rclpy.executors.WaitableEntityType) -> None: 62 | """Notify group that a callback has finished executing. 63 | 64 | Args: 65 | entity: A subscription, timer, client, service, or waitable instance. 66 | """ 67 | with self._lock: 68 | assert entity in self._active_entities 69 | self._active_entities.remove(entity) 70 | -------------------------------------------------------------------------------- /synchros2/synchros2/context.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import threading 3 | from typing import Optional 4 | 5 | from rclpy.context import Context 6 | from rclpy.utilities import get_default_context 7 | 8 | 9 | def wait_for_shutdown(*, timeout_sec: Optional[float] = None, context: Optional[Context] = None) -> bool: 10 | """Wait for context shutdown. 11 | 12 | Args: 13 | timeout_sec: optional timeout for wait, wait indefinitely by default. 14 | context: context to wait on, use default context by default. 15 | 16 | Returns: 17 | True if shutdown, False on timeout. 18 | """ 19 | if context is None: 20 | context = get_default_context() 21 | event = threading.Event() 22 | context.on_shutdown(event.set) 23 | return event.wait(timeout_sec) 24 | -------------------------------------------------------------------------------- /synchros2/synchros2/futures.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from threading import Event 3 | from typing import Any, Awaitable, Callable, Optional, Protocol, TypeVar, Union, runtime_checkable 4 | 5 | from rclpy.context import Context 6 | from rclpy.utilities import get_default_context 7 | 8 | T = TypeVar("T", covariant=True) 9 | 10 | 11 | class FutureLike(Awaitable[T], Protocol[T]): 12 | """A future-like awaitable object. 13 | 14 | Matches `rclpy.task.Future` and `concurrent.futures.Future` protocols. 15 | """ 16 | 17 | def result(self) -> T: 18 | """Get future result (may block).""" 19 | ... 20 | 21 | def exception(self) -> Optional[Exception]: 22 | """Get future exception, if any.""" 23 | ... 24 | 25 | def done(self) -> bool: 26 | """Check if future is ready.""" 27 | ... 28 | 29 | def add_done_callback(self, func: Callable[["FutureLike[T]"], None]) -> None: 30 | """Add a callback to be scheduled as soon as the future is ready.""" 31 | ... 32 | 33 | def cancel(self) -> None: 34 | """Cancel future.""" 35 | ... 36 | 37 | def cancelled(self) -> bool: 38 | """Check if future was cancelled.""" 39 | ... 40 | 41 | 42 | @runtime_checkable 43 | class FutureConvertible(Awaitable[T], Protocol[T]): 44 | """An awaitable that is convertible to a future-like object.""" 45 | 46 | def as_future(self) -> FutureLike[T]: 47 | """Get future-like view.""" 48 | ... 49 | 50 | 51 | AnyFuture = Union[FutureLike, FutureConvertible] 52 | 53 | 54 | def as_proper_future(instance: AnyFuture) -> FutureLike: 55 | """Return `instance` as a proper future-like object.""" 56 | if isinstance(instance, FutureConvertible): 57 | return instance.as_future() 58 | return instance 59 | 60 | 61 | def wait_for_future( 62 | future: AnyFuture, 63 | timeout_sec: Optional[float] = None, 64 | *, 65 | context: Optional[Context] = None, 66 | ) -> bool: 67 | """Block while waiting for a future to become done 68 | 69 | Args: 70 | future (Future): The future to be waited on 71 | timeout_sec (Optional[float]): An optional timeout for how long to wait 72 | context (Optional[Context]): Current context (will use the default if none is given) 73 | 74 | Returns: 75 | bool: True if successful, False if the timeout was triggered 76 | """ 77 | if context is None: 78 | context = get_default_context() 79 | event = Event() 80 | context.on_shutdown(event.set) 81 | proper_future = as_proper_future(future) 82 | proper_future.add_done_callback(lambda _: event.set()) 83 | if proper_future.cancelled(): 84 | event.set() 85 | event.wait(timeout=timeout_sec) 86 | return proper_future.done() 87 | 88 | 89 | def unwrap_future( 90 | future: AnyFuture, 91 | timeout_sec: Optional[float] = None, 92 | *, 93 | context: Optional[Context] = None, 94 | ) -> Any: 95 | """Fetch future result when it is done. 96 | 97 | Note this function may block and may raise if the future does or it times out 98 | waiting for it. See wait_for_future() documentation for further reference on 99 | arguments taken. 100 | """ 101 | proper_future = as_proper_future(future) 102 | if not wait_for_future(proper_future, timeout_sec, context=context): 103 | raise ValueError("cannot unwrap future that is not done") 104 | return proper_future.result() 105 | -------------------------------------------------------------------------------- /synchros2/synchros2/graph.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import functools 3 | from typing import Callable, Optional 4 | 5 | from rclpy.node import Node 6 | from rclpy.task import Future 7 | 8 | from synchros2.futures import unwrap_future 9 | 10 | 11 | def _ensure_num_matched_async( 12 | node: Node, 13 | lower_bound: int, 14 | num_matched_func: Callable[[], int], 15 | *, 16 | poll_period_sec: float = 0.1, 17 | ) -> Future: 18 | """Returns a future to the number of matched elements based a user-provided function. 19 | 20 | Note this method relies on polling. It is frequently used to count the number of publishers or subscriptions on a 21 | topic and in ROS 2 Humble and earlier distributions matching events are missing. 22 | 23 | Args: 24 | node: A ROS 2 node 25 | lower_bound: The lower bound on the number of matched elements 26 | num_matched_func: The function to use to count the number of matched elements 27 | poll_period_sec: The period for polling the count 28 | 29 | Returns: 30 | A future to the number of matched elements 31 | """ 32 | future = Future() 33 | num_matched = num_matched_func() 34 | if num_matched < lower_bound: 35 | 36 | def _poll() -> None: 37 | nonlocal future, lower_bound 38 | if future.cancelled(): 39 | return 40 | num_matched: int = num_matched_func() 41 | assert num_matched is not None 42 | if lower_bound <= num_matched: 43 | future.set_result(num_matched) 44 | 45 | timer = node.create_timer(poll_period_sec, _poll) 46 | future.add_done_callback(lambda _: node.destroy_timer(timer)) 47 | else: 48 | future.set_result(num_matched) 49 | return future 50 | 51 | 52 | def _ensure_num_matched( 53 | node: Node, 54 | lower_bound: int, 55 | num_matched_func: Callable[[], int], 56 | timeout_sec: Optional[float] = None, 57 | *, 58 | poll_period_sec: float = 0.1, 59 | ) -> int: 60 | """Returns the number of matched elements based a user-provided function. 61 | 62 | Note this method relies on polling. It is frequently used to count the number of publishers or subscriptions on a 63 | topic and in ROS 2 Humble and earlier distributions matching events are missing. 64 | 65 | Args: 66 | node: A ROS 2 node 67 | lower_bound: The lower bound on the number of matched elements 68 | num_matched_func: The function to use to count the number of matched elements 69 | timeout_sec: A timeout on how long to wait for the lower bound to be satisfied 70 | poll_period_sec: The period for polling the count 71 | 72 | Returns: 73 | The number of matched elements 74 | 75 | Exceptions: 76 | TimeoutError if the lower bound has not been satisfied in `timeout_sec` seconds. 77 | """ 78 | future = _ensure_num_matched_async( 79 | node=node, 80 | lower_bound=lower_bound, 81 | num_matched_func=num_matched_func, 82 | poll_period_sec=poll_period_sec, 83 | ) 84 | return unwrap_future(future, timeout_sec=timeout_sec) 85 | 86 | 87 | def ensure_num_publishers_async( 88 | node: Node, 89 | topic_name: str, 90 | num_publishers: int, 91 | *, 92 | poll_period_sec: float = 0.1, 93 | ) -> Future: 94 | """Returns a future to the number of publishers on a topic. 95 | 96 | Note that in ROS 2 Humble and earlier distributions, this method relies on 97 | polling the number of known publishers for the topic subscribed, as subscription 98 | matching events are missing. 99 | 100 | Args: 101 | node: A ROS 2 node 102 | topic_name: The name of the topic to check 103 | num_publishers: The lower bound on the number of publishers to match. 104 | timeout_sec: A timeout on how long to wait for the lower bound to be satisfied 105 | poll_period_sec: The period for polling the count 106 | 107 | Returns: 108 | A future to the number of publishers 109 | """ 110 | return _ensure_num_matched_async( 111 | node=node, 112 | lower_bound=num_publishers, 113 | num_matched_func=functools.partial(node.count_publishers, topic_name), 114 | poll_period_sec=poll_period_sec, 115 | ) 116 | 117 | 118 | def ensure_num_publishers( 119 | node: Node, 120 | topic_name: str, 121 | num_publishers: int, 122 | timeout_sec: Optional[float] = None, 123 | *, 124 | poll_period_sec: float = 0.1, 125 | ) -> int: 126 | """Returns the number of publishers on a topic. 127 | 128 | Note that in ROS 2 Humble and earlier distributions, this method relies on 129 | polling the number of known publishers for the topic subscribed, as subscription 130 | matching events are missing. 131 | 132 | Args: 133 | node: A ROS 2 node 134 | topic_name: The name of the topic to check 135 | num_publishers: The lower bound on the number of publishers to match. 136 | timeout_sec: A timeout on how long to wait for the lower bound to be satisfied 137 | poll_period_sec: The period for polling the count 138 | 139 | Returns: 140 | The number of publishers 141 | """ 142 | return _ensure_num_matched( 143 | node=node, 144 | lower_bound=num_publishers, 145 | num_matched_func=functools.partial(node.count_publishers, topic_name), 146 | timeout_sec=timeout_sec, 147 | poll_period_sec=poll_period_sec, 148 | ) 149 | 150 | 151 | def ensure_num_subscriptions_async( 152 | node: Node, 153 | topic_name: str, 154 | num_subscriptions: int, 155 | *, 156 | poll_period_sec: float = 0.1, 157 | ) -> Future: 158 | """Returns a future to the number of subscriptions on a topic. 159 | 160 | Note that in ROS 2 Humble and earlier distributions, this method relies on 161 | polling the number of known subscriptions for the topic subscribed, as subscription 162 | matching events are missing. 163 | 164 | Args: 165 | node: A ROS 2 node 166 | topic_name: The name of the topic to check 167 | num_subscriptions: The lower bound on the number of subscriptions to match. 168 | timeout_sec: A timeout on how long to wait for the lower bound to be satisfied 169 | poll_period_sec: The period for polling the count 170 | 171 | Returns: 172 | A future to the number of publishers 173 | """ 174 | return _ensure_num_matched_async( 175 | node=node, 176 | lower_bound=num_subscriptions, 177 | num_matched_func=functools.partial(node.count_subscribers, topic_name), 178 | poll_period_sec=poll_period_sec, 179 | ) 180 | 181 | 182 | def ensure_num_subscriptions( 183 | node: Node, 184 | topic_name: str, 185 | num_subscriptions: int, 186 | timeout_sec: Optional[float] = None, 187 | *, 188 | poll_period_sec: float = 0.1, 189 | ) -> int: 190 | """Returns the number of subscriptions on a topic. 191 | 192 | Note that in ROS 2 Humble and earlier distributions, this method relies on 193 | polling the number of known subscriptions for the topic subscribed, as subscription 194 | matching events are missing. 195 | 196 | Args: 197 | node: A ROS 2 node 198 | topic_name: The name of the topic to check 199 | num_subscriptions: The lower bound on the number of subscriptions to match. 200 | timeout_sec: A timeout on how long to wait for the lower bound to be satisfied 201 | poll_period_sec: The period for polling the count 202 | 203 | Returns: 204 | The number of subscriptions 205 | """ 206 | return _ensure_num_matched( 207 | node=node, 208 | lower_bound=num_subscriptions, 209 | num_matched_func=functools.partial(node.count_subscribers, topic_name), 210 | timeout_sec=timeout_sec, 211 | poll_period_sec=poll_period_sec, 212 | ) 213 | -------------------------------------------------------------------------------- /synchros2/synchros2/launch/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | -------------------------------------------------------------------------------- /synchros2/synchros2/launch/actions.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from __future__ import annotations 3 | 4 | from enum import Enum 5 | from typing import Any, Final, List, Type 6 | 7 | from launch import LaunchDescription 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.utilities.type_utils import coerce_to_type 10 | 11 | _BOOLEAN_STR_CHOICES: Final[List[str]] = ["true", "True", "false", "False"] 12 | _BOOLEAN_CHOICES: Final[List[str | bool]] = [*_BOOLEAN_STR_CHOICES, True, False] 13 | _OPTIONAL_CHOICES: Final[List[str]] = [""] 14 | 15 | 16 | def convert_to_bool(param_name: str, val: str) -> bool: 17 | """Converts a ros parameter to a bool""" 18 | try: 19 | return coerce_to_type(val.lower(), bool) 20 | except ValueError: 21 | print(f"Cannot convert `{param_name}` to bool (value is {val})") 22 | raise 23 | 24 | 25 | def update_sigterm_sigkill_timeout( 26 | ld: LaunchDescription, 27 | *, 28 | sigterm_timeout_s: float = 60, 29 | sigkill_timeout_s: float = 60, 30 | ) -> None: 31 | """Increases the timeout for launch to escalate to SIGTERM and SIGKILL after you CTRL+C""" 32 | ld.add_action(DeclareLaunchArgument("sigterm_timeout", default_value=str(sigterm_timeout_s))) 33 | ld.add_action(DeclareLaunchArgument("sigkill_timeout", default_value=str(sigkill_timeout_s))) 34 | 35 | 36 | class DeclareBooleanLaunchArgument(DeclareLaunchArgument): 37 | """Thin wrapper on `DeclareLaunchArgument` to restrict the choices to boolean""" 38 | 39 | def __init__(self, *args: Any, **kwargs: Any) -> None: 40 | if "choices" in kwargs: 41 | raise KeyError("Cannot set `choices` for `DeclareBooleanLaunchArgument`") 42 | if "default_value" in kwargs: 43 | default_value = kwargs["default_value"] 44 | if default_value not in _BOOLEAN_CHOICES: 45 | raise ValueError(f"`default_value` must be from {_BOOLEAN_CHOICES}") 46 | if isinstance(default_value, bool): 47 | kwargs["default_value"] = "true" if default_value else "false" 48 | 49 | super().__init__(*args, choices=_BOOLEAN_STR_CHOICES, **kwargs) 50 | 51 | 52 | class DeclareEnumLaunchArgument(DeclareLaunchArgument): 53 | """Thin wrapper on `DeclareLaunchArgument` to restrict the choices to the values of an enum""" 54 | 55 | def __init__(self, enum_type: Type[Enum], *args: Any, optional: bool = False, **kwargs: Any) -> None: 56 | choices = [str(e.value) for e in enum_type] + ( 57 | [] if not optional else _OPTIONAL_CHOICES 58 | ) # typing: ignore[attr-defined] 59 | if "choices" in kwargs: 60 | raise KeyError("Cannot set `choices` for `DeclareEnumLaunchArgument`") 61 | if "default_value" in kwargs: 62 | default_value = kwargs["default_value"] 63 | if isinstance(default_value, str): 64 | if default_value not in choices: 65 | raise ValueError( 66 | ( 67 | f"For an Enum Launch Argument of type {enum_type.__name__}, the `default_value` must be" 68 | f" from {choices} or {list(enum_type)}" 69 | ), 70 | ) 71 | elif isinstance(default_value, enum_type): 72 | if default_value not in enum_type: 73 | raise ValueError( 74 | ( 75 | f"For an Enum Launch Argument of type {enum_type.__name__}, the `default_value` must be" 76 | f" from {choices} or {list(enum_type)}" 77 | ), 78 | ) 79 | kwargs["default_value"] = str(default_value.value) 80 | else: 81 | raise TypeError( 82 | ( 83 | f"For an Enum Launch Argument of type {enum_type.__name__}, the `default_value` must be of type" 84 | f" `str` or `{enum_type.__name__}`" 85 | ), 86 | ) 87 | 88 | super().__init__(*args, choices=choices, **kwargs) 89 | -------------------------------------------------------------------------------- /synchros2/synchros2/launch/arguments.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from __future__ import annotations 3 | 4 | from typing import Literal 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | from synchros2.launch.actions import DeclareBooleanLaunchArgument 11 | 12 | _ROBOT_NAME: Literal["robot_name"] = "robot_name" 13 | _VERBOSE: Literal["verbose"] = "verbose" 14 | 15 | 16 | def add_robot_name_argument(ld: LaunchDescription) -> LaunchConfiguration: 17 | """Adds a launch argument for `robot_name` to the `LaunchDescription` 18 | 19 | Args: 20 | ld: The launch description instance 21 | 22 | Returns: 23 | A launch configuration that can be used to parse the command line argument for `robot_name` 24 | """ 25 | ld.add_action( 26 | DeclareLaunchArgument( 27 | _ROBOT_NAME, 28 | description="Name of the robot.", 29 | default_value="", 30 | ), 31 | ) 32 | return LaunchConfiguration(_ROBOT_NAME) 33 | 34 | 35 | def add_verbose_argument(ld: LaunchDescription) -> LaunchConfiguration: 36 | """Adds a launch argument for `verbose` to the `LaunchDescription` 37 | 38 | Args: 39 | ld: The launch description instance 40 | 41 | Returns: 42 | A launch configuration that can be used to parse the command line argument for `verbose` 43 | """ 44 | ld.add_action(DeclareBooleanLaunchArgument(_VERBOSE, description="Run with debug logging on.", default_value=False)) 45 | return LaunchConfiguration(_VERBOSE) 46 | -------------------------------------------------------------------------------- /synchros2/synchros2/launch/substitutions.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from typing import List, Optional 3 | 4 | from launch import Condition, SomeSubstitutionsType 5 | from launch.conditions import UnlessCondition 6 | from launch.substitutions import OrSubstitution 7 | 8 | 9 | # TODO: when/if we move to rolling we should use the `AnySubstitution` 10 | def not_any_substitution(conditions: List[SomeSubstitutionsType]) -> Optional[Condition]: 11 | """A substitution that is True if none of the conditions are substituted with True 12 | 13 | Args: 14 | conditions (list[SomeSubstitutionsType]): A list of substitutions 15 | Returns: 16 | Optional[Condition]: A substitution that is True if none of the conditions are substituted with True, if less 17 | than 2 conditions are provided returns none 18 | """ 19 | if len(conditions) < 2: 20 | print("Bad number of conditions in not_any_substitution.") 21 | return None 22 | 23 | substitution = OrSubstitution(conditions[0], conditions[1]) 24 | for cond in conditions[2:]: 25 | substitution = OrSubstitution(substitution, cond) 26 | 27 | return UnlessCondition(substitution) 28 | -------------------------------------------------------------------------------- /synchros2/synchros2/launch/values.py: -------------------------------------------------------------------------------- 1 | """Provides a helper class for accessing the values of launch arguments from a LaunchConfiguration 2 | 3 | Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 4 | """ 5 | # mypy: ignore-errors 6 | from launch import LaunchContext 7 | from launch.substitutions import LaunchConfiguration 8 | 9 | from synchros2.launch.actions import convert_to_bool 10 | 11 | 12 | class LaunchConfigurationValues: 13 | """Helper class for accessing the values of launch arguments from a LaunchConfiguration. 14 | 15 | This helper serves the following purposes: 16 | 1. Avoid spending a line of code on each of a potentially large number of values retrieved from the launch config 17 | 2. Enable easily accessing the values of boolean arguments as actual `bool` types when needed, 18 | while still normally treating them as `str` the way downstream launch operations usually expect. 19 | 20 | Example: 21 | ---------------------------------------------------------------- 22 | def launch_setup(context: LaunchContext): 23 | launch_elements = [] 24 | vals = LaunchConfigurationValues(context) 25 | vals["robot_name"] # str 26 | vals.bool("use_afterburner") # bool 27 | 28 | # The "condition" kwarg expects a launch operation on a string-type boolean, not an actual bool 29 | launch_elements.append( 30 | Node( 31 | [...] 32 | condition=IfCondition(vals["use_afterburner"]), # Argument is in ["true", "false"], not [True, False] 33 | ), 34 | ) 35 | 36 | if vals.bool("use_afterburner"): # Access value as one of [True, False] 37 | # Add an additional launch element 38 | [...] 39 | ---------------------------------------------------------------- 40 | """ 41 | 42 | def __init__(self, context: LaunchContext): 43 | """Initialize a LaunchConfigurationValues helper for a given launch context 44 | 45 | Args: 46 | context : The LaunchContext being used in the launch_setup function where this helper class is needed. 47 | """ 48 | self._context = context 49 | self._string_values: dict[str, str] = {} 50 | self._bool_values: dict[str, bool] = {} 51 | 52 | def __getitem__(self, launch_arg_name: str) -> str: 53 | """Retrieve the string value of the specified launch argument""" 54 | if launch_arg_name not in self._string_values: 55 | self._string_values[launch_arg_name] = LaunchConfiguration(launch_arg_name).perform(self._context) 56 | return self._string_values[launch_arg_name] 57 | 58 | def bool(self, launch_arg_name: str) -> bool: # noqa: A003 59 | """Retrieve the boolean value of the specified launch argument. 60 | 61 | This method should fail if the argument's string value is not one of the expected options for a boolean. 62 | """ 63 | if launch_arg_name not in self._bool_values: 64 | self._bool_values[launch_arg_name] = convert_to_bool(launch_arg_name, self.__getitem__(launch_arg_name)) 65 | return self._bool_values[launch_arg_name] 66 | -------------------------------------------------------------------------------- /synchros2/synchros2/node.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import contextlib 3 | import functools 4 | from typing import Any, Callable, Iterable, Optional, Type 5 | 6 | from rclpy.callback_groups import CallbackGroup 7 | from rclpy.exceptions import InvalidHandle 8 | from rclpy.node import Node as BaseNode 9 | from rclpy.waitable import Waitable 10 | 11 | from synchros2.callback_groups import NonReentrantCallbackGroup 12 | from synchros2.logging import MemoizingRcutilsLogger, as_memoizing_logger 13 | 14 | 15 | def suppressed(exception: Type[BaseException], func: Callable) -> Callable: 16 | """Suppress the given `exception` type from `func` invocations""" 17 | 18 | @functools.wraps(func) 19 | def __wrapper(*args: Any, **kwargs: Any) -> Any: 20 | with contextlib.suppress(exception): 21 | return func(*args, **kwargs) 22 | 23 | return __wrapper 24 | 25 | 26 | class Node(BaseNode): 27 | """An rclpy.node.Node subclass that: 28 | 29 | * changes the default callback group to be non-reentrant 30 | * wraps its logger with a memoizing one for improved efficiency 31 | """ 32 | 33 | def __init__(self, *args: Any, default_callback_group: Optional[CallbackGroup] = None, **kwargs: Any) -> None: 34 | """Initializes the node. 35 | 36 | Args: 37 | args: positional arguments for a ros Node 38 | default_callback_group: optional callback group to use as default 39 | for all subsequently created entities, such as subscriptions 40 | and clients. 41 | kwargs: keyword arguments for a ros Node 42 | 43 | See rclpy.node.Node documentation for further reference on available arguments. 44 | """ 45 | if default_callback_group is None: 46 | default_callback_group = NonReentrantCallbackGroup() 47 | self._default_callback_group_override = default_callback_group 48 | self._destruction_requested = False 49 | super().__init__(*args, **kwargs) 50 | self._logger: MemoizingRcutilsLogger = as_memoizing_logger(self._logger) 51 | 52 | @property 53 | def default_callback_group(self) -> CallbackGroup: 54 | """Get the default callback group.""" 55 | # NOTE(hidmic): this overrides the hardcoded default group in rclpy.node.Node implementation 56 | return self._default_callback_group_override 57 | 58 | @property 59 | def waitables(self) -> Iterable[Waitable]: 60 | """Get patched node waitables. 61 | 62 | Workaround for https://github.com/ros2/rclpy/issues/1284. 63 | """ 64 | for waitable in super().waitables: 65 | if not getattr(waitable, "__patched__", False): 66 | waitable.add_to_wait_set = suppressed(InvalidHandle, waitable.add_to_wait_set) 67 | waitable.is_ready = suppressed(IndexError, waitable.is_ready) 68 | waitable.__patched__ = True 69 | yield waitable 70 | 71 | @property 72 | def destruction_requested(self) -> bool: 73 | """Checks whether destruction was requested or not.""" 74 | return self._destruction_requested 75 | 76 | def destroy_node(self) -> None: 77 | """Overrides node destruction API.""" 78 | self._destruction_requested = True 79 | super().destroy_node() 80 | -------------------------------------------------------------------------------- /synchros2/synchros2/publisher.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Boston Dynamics AI Institute Inc. All rights reserved. 2 | 3 | from asyncio import Future 4 | from typing import Any, Generic, Optional, Type, TypeVar 5 | 6 | import rclpy.publisher 7 | from rclpy.node import Node 8 | 9 | import synchros2.scope as scope 10 | 11 | # Represents a ros message type 12 | _MessageT = TypeVar("_MessageT") 13 | 14 | 15 | class Publisher(Generic[_MessageT]): 16 | """An extension of a publisher from ROS 2.""" 17 | 18 | def __init__(self, *args: Any, node: Optional[Node] = None, **kwargs: Any) -> None: 19 | """Initializes the Publisher 20 | 21 | Args: 22 | args: Positional arguments to pass to the `Node.create_publisher` function 23 | node: Optional node for the underlying native subscription, defaults to 24 | the current process node. 25 | kwargs: Keyword arguments to pass to the `Node.create_publisher` function 26 | """ 27 | if node is None: 28 | node = scope.ensure_node() 29 | self._node = node 30 | self._publisher = self._node.create_publisher(*args, **kwargs) 31 | 32 | def subscription_matches(self, num_subscriptions: int) -> Future: 33 | """Gets a future to next publisher matching status update. 34 | 35 | Note that in ROS 2 Humble and earlier distributions, this method relies on 36 | polling the number of known subscriptions for the topic subscribed, as publisher 37 | matching events are missing. 38 | 39 | Args: 40 | num_subscriptions: lower bound on the number of subscriptions to match. 41 | 42 | Returns: 43 | a future, done if the current number of subscriptions already matches 44 | the specified lower bound. 45 | """ 46 | future_match = Future() # type: ignore[var-annotated] 47 | num_matched_publishers = self._node.count_subscribers(self._publisher.topic_name) 48 | if num_matched_publishers < num_subscriptions: 49 | 50 | def _poll_publisher_matches() -> None: 51 | nonlocal future_match, num_subscriptions 52 | if future_match.cancelled(): 53 | return 54 | num_matched_subscriptions = self._node.count_subscribers(self._publisher.topic_name) 55 | if num_subscriptions <= num_matched_subscriptions: 56 | future_match.set_result(num_matched_subscriptions) 57 | 58 | timer = self._node.create_timer(0.1, _poll_publisher_matches) 59 | future_match.add_done_callback(lambda _: self._node.destroy_timer(timer)) 60 | else: 61 | future_match.set_result(num_matched_publishers) 62 | return future_match 63 | 64 | @property 65 | def matched_subscriptions(self) -> int: 66 | """Gets the number subscriptions matched and linked to. 67 | 68 | Note that in ROS 2 Humble and earlier distributions, this property 69 | relies on the number of known subscriptions for the topic subscribed 70 | as subscription matching status info is missing. 71 | """ 72 | return self._node.count_subscribers(self._publisher.topic_name) 73 | 74 | @property 75 | def message_type(self) -> Type[_MessageT]: 76 | """Gets the type of the message subscribed.""" 77 | return self._publisher.msg_type 78 | 79 | def publisher(self) -> rclpy.publisher.Publisher: 80 | """Returns the internal ROS 2 publisher""" 81 | return self._publisher 82 | 83 | def __getattr__(self, name: str) -> Any: 84 | return getattr(self._publisher, name) 85 | -------------------------------------------------------------------------------- /synchros2/synchros2/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/ros_utilities/bbe365dfafef15e9a6379246f801aca4d90a932b/synchros2/synchros2/py.typed -------------------------------------------------------------------------------- /synchros2/synchros2/service.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from typing import Any, Generic, Optional, Protocol, Type, TypeVar, overload 4 | 5 | from rclpy.client import Client 6 | from rclpy.node import Node 7 | from rclpy.task import Future 8 | 9 | import synchros2.scope as scope 10 | from synchros2.callables import ComposableCallable, VectorizingCallable 11 | from synchros2.futures import FutureLike, wait_for_future 12 | 13 | 14 | class ServiceException(Exception): 15 | """Base service exception.""" 16 | 17 | def __init__(self, service: Future) -> None: 18 | super().__init__(service) 19 | self.service = service 20 | 21 | def __str__(self) -> str: 22 | return "unknown service error" 23 | 24 | 25 | class ServiceTimeout(ServiceException): 26 | """Exception raised on service timeout.""" 27 | 28 | def __str__(self) -> str: 29 | return "service timed out" 30 | 31 | 32 | class ServiceError(ServiceException): 33 | """Exception raised on service error.""" 34 | 35 | def __str__(self) -> str: 36 | exception = self.service.exception() 37 | if exception is not None: 38 | return f"service failed (due to {exception})" 39 | result = self.service.result() 40 | if hasattr(result, "message"): 41 | return f"service failed (due to {result.message})" 42 | return "service failed" 43 | 44 | 45 | ServiceRequestT = TypeVar("ServiceRequestT", contravariant=True) 46 | ServiceResponseT = TypeVar("ServiceResponseT", covariant=True) 47 | 48 | 49 | class ServicedProtocol(Protocol[ServiceRequestT, ServiceResponseT]): 50 | """Ergonomic protocol to call services in ROS 2.""" 51 | 52 | @property 53 | def client(self) -> Client: 54 | """Get the underlying service client.""" 55 | 56 | def wait_for_service(self, *args: Any, **kwargs: Any) -> bool: 57 | """Wait for service to become available.""" 58 | 59 | @overload 60 | def synchronous( 61 | self, 62 | request: Optional[ServiceRequestT] = ..., 63 | *, 64 | timeout_sec: Optional[float] = ..., 65 | ) -> ServiceResponseT: 66 | """Invoke service synchronously. 67 | 68 | Args: 69 | request: request to be serviced, or a default initialized one if none is provided. 70 | timeout_sec: optional action timeout, in seconds. If a timeout is specified and it 71 | expires, the service request will be cancelled and the call will raise. Note this 72 | timeout is local to the caller. 73 | 74 | Returns: 75 | the service response. 76 | 77 | Raises: 78 | ServiceTimeout: if the service request timed out. 79 | ServiceError: if the service request failed. 80 | """ 81 | 82 | @overload 83 | def synchronous( 84 | self, 85 | request: Optional[ServiceRequestT] = ..., 86 | *, 87 | timeout_sec: Optional[float] = ..., 88 | nothrow: bool = ..., 89 | ) -> Optional[ServiceResponseT]: 90 | """Invoke service synchronously. 91 | 92 | Args: 93 | request: request to be serviced, or a default initialized one if none is provided. 94 | timeout_sec: optional action timeout, in seconds. If a timeout is specified and it 95 | expires, the service request will be cancelled and the call will raise. Note this 96 | timeout is local to the caller. 97 | nothrow: when set, errors do not raise exceptions. 98 | 99 | Returns: 100 | the service response or None on timeout or failure. 101 | """ 102 | 103 | def asynchronous(self, request: Optional[ServiceRequestT] = ...) -> FutureLike[ServiceResponseT]: 104 | """Invoke service asynchronously. 105 | 106 | Args: 107 | request: request to be serviced, or a default initialized one if none is provided. 108 | 109 | Returns: 110 | the future response. 111 | """ 112 | 113 | 114 | class Serviced(Generic[ServiceRequestT, ServiceResponseT], ComposableCallable, VectorizingCallable): 115 | """An ergonomic interface to call services in ROS 2. 116 | 117 | Serviced instances wrap `rclpy.Client` instances to allow for synchronous 118 | and asynchronous service invocations, in a way that resembles remote 119 | procedure calls. 120 | """ 121 | 122 | def __init__(self, service_type: Type, service_name: str, node: Optional[Node] = None, **kwargs: Any) -> None: 123 | if node is None: 124 | node = scope.ensure_node() 125 | self._service_type = service_type 126 | self._service_name = service_name 127 | self._client = node.create_client(service_type, service_name, **kwargs) 128 | 129 | @property 130 | def service_name(self) -> str: 131 | """Get the target service name.""" 132 | return self._service_name 133 | 134 | @property 135 | def service_type(self) -> Type: 136 | """Get the target service type.""" 137 | return self._service_type 138 | 139 | @property 140 | def client(self) -> Client: 141 | """Get the underlying service client.""" 142 | return self._client 143 | 144 | def wait_for_service(self, *args: Any, **kwargs: Any) -> bool: 145 | """Wait for service to become available. 146 | 147 | See `rclpy.Client.wait_for_service()` documentation for further reference. 148 | """ 149 | return self._client.wait_for_service(*args, **kwargs) 150 | 151 | @overload 152 | def synchronous( 153 | self, 154 | request: Optional[ServiceRequestT] = ..., 155 | *, 156 | timeout_sec: Optional[float] = ..., 157 | ) -> ServiceResponseT: 158 | """Invoke service synchronously. 159 | 160 | Args: 161 | request: request to be serviced, or a default initialized one if none is provided. 162 | timeout_sec: optional action timeout, in seconds. If a timeout is specified and it 163 | expires, the service request will be cancelled and the call will raise. Note this 164 | timeout is local to the caller. 165 | 166 | Returns: 167 | the service response. 168 | 169 | Raises: 170 | ServiceTimeout: if the service request timed out. 171 | ServiceError: if the service request failed. 172 | """ 173 | 174 | @overload 175 | def synchronous( 176 | self, 177 | request: Optional[ServiceRequestT] = ..., 178 | *, 179 | timeout_sec: Optional[float] = ..., 180 | nothrow: bool = ..., 181 | ) -> Optional[ServiceResponseT]: 182 | """Invoke service synchronously but never raise a exception. 183 | 184 | Args: 185 | request: request to be serviced, or a default initialized one if none is provided. 186 | timeout_sec: optional action timeout, in seconds. If a timeout is specified and it 187 | expires, the service request will be cancelled and the call will raise. Note this 188 | timeout is local to the caller. 189 | nothrow: when set, errors do not raise exceptions. 190 | 191 | Returns: 192 | the service response or None on timeout or error. 193 | """ 194 | 195 | def synchronous( 196 | self, 197 | request: Optional[ServiceRequestT] = None, 198 | *, 199 | timeout_sec: Optional[float] = None, 200 | nothrow: bool = False, 201 | ) -> Optional[ServiceResponseT]: 202 | """Invoke service synchronously. 203 | 204 | Check available overloads documentation. 205 | """ 206 | future = self.asynchronous(request) 207 | if not wait_for_future(future, timeout_sec): 208 | future.cancel() 209 | if nothrow: 210 | return None 211 | raise ServiceTimeout(future) 212 | exception = future.exception() 213 | if exception is not None: 214 | if nothrow: 215 | return None 216 | raise ServiceError(future) from exception 217 | response = future.result() 218 | if not nothrow and not getattr(response, "success", True): 219 | raise ServiceError(future) 220 | return response 221 | 222 | def asynchronous(self, request: Optional[ServiceRequestT] = None) -> FutureLike[ServiceResponseT]: 223 | """Invoke service asynchronously. 224 | 225 | Args: 226 | request: request to be serviced. If none is provided, a default 227 | initialized request will be used instead. 228 | 229 | Returns: 230 | the future response. 231 | """ 232 | if request is None: 233 | request = self.service_type.Request() 234 | return self._client.call_async(request) 235 | -------------------------------------------------------------------------------- /synchros2/synchros2/service_handle.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import threading 3 | from typing import Any, Callable, Optional 4 | 5 | from rclpy.context import Context 6 | from rclpy.impl.rcutils_logger import RcutilsLogger 7 | from rclpy.task import Future 8 | from rclpy.utilities import get_default_context 9 | 10 | 11 | class ServiceHandle: 12 | """A handle for the lifecycle of a service. 13 | 14 | Handles getting a result after sending an ServiceRequest to Service 15 | as holding the various callbacks for sending an ServiceRequest (result, failure) 16 | """ 17 | 18 | def __init__(self, service_name: str, logger: Optional[RcutilsLogger] = None, context: Optional[Context] = None): 19 | """Constructor 20 | 21 | Args: 22 | service_name: The name of the service handle to be used in logging 23 | logger: An optional ros logger 24 | context: An optional ros context 25 | """ 26 | if context is None: 27 | context = get_default_context() 28 | self._service_name = service_name 29 | self._send_service_future: Optional[Future] = None 30 | self._future_ready_event = threading.Event() 31 | context.on_shutdown(self._future_ready_event.set) 32 | self._result_callback: Optional[Callable] = None 33 | self._on_failure_callback: Optional[Callable] = None 34 | self._result: Optional[Any] = None 35 | if logger is None: 36 | self._logger = RcutilsLogger(name=f"{service_name} Handle") 37 | else: 38 | self._logger = logger 39 | 40 | @property 41 | def result(self) -> Optional[Any]: 42 | """Returns the result if one has been received from the service""" 43 | return self._result 44 | 45 | def set_result_callback(self, result_callback: Callable) -> None: 46 | """Sets the callback for when the future.result() completes.""" 47 | self._result_callback = result_callback 48 | 49 | def set_send_service_future(self, send_service_future: Future) -> None: 50 | """Sets the future received from sending the Action.Goal""" 51 | self._send_service_future = send_service_future 52 | self._send_service_future.add_done_callback(self._service_result_callback) 53 | 54 | def set_on_failure_callback(self, on_failure_callback: Callable) -> None: 55 | """Set the callback to execute upon failure""" 56 | self._on_failure_callback = on_failure_callback 57 | 58 | def wait(self, timeout_sec: Optional[float] = None) -> bool: 59 | """Wait for service response, if any.""" 60 | if self._send_service_future is None: 61 | raise RuntimeError("no future to wait on") 62 | return self._future_ready_event.wait(timeout_sec) 63 | 64 | def _service_result_callback(self, future: Future) -> None: 65 | """Callback that handles receiving a response from the Service""" 66 | result = future.result() 67 | 68 | if result is None: 69 | self._logger.error(f"Service request failed: {future.exception():!r}") 70 | self._failure() 71 | self._future_ready_event.set() 72 | return 73 | 74 | self._result = result 75 | 76 | if self._result_callback is not None: 77 | self._result_callback(result) 78 | 79 | # If there's a failure callback but no success case for result, warn and return 80 | service_has_success = hasattr(result, "success") 81 | 82 | if self._on_failure_callback is not None and not service_has_success: 83 | self._logger.warning("Failure callback is set, but result has no success attribute") 84 | self._future_ready_event.set() 85 | return 86 | 87 | if service_has_success: 88 | if not result.success: 89 | self._logger.error(f"Service failed; error is {result.message}") 90 | self._failure() 91 | self._future_ready_event.set() 92 | return 93 | else: 94 | self._logger.warning(f"Service {self._service_name} has no success or failure flag") 95 | 96 | self._logger.info("Service completed") 97 | self._future_ready_event.set() 98 | 99 | def _failure(self) -> None: 100 | """Triggers the internal failure callback if it exists""" 101 | if self._on_failure_callback is not None: 102 | self._on_failure_callback() 103 | -------------------------------------------------------------------------------- /synchros2/synchros2/single_goal_action_server.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from typing import Callable, Optional 4 | 5 | from rclpy.callback_groups import CallbackGroup 6 | from rclpy.node import Node 7 | 8 | from synchros2.single_goal_multiple_action_servers import SingleGoalMultipleActionServers 9 | from synchros2.type_hints import ActionType 10 | 11 | # Note: for this to work correctly you must use a multi-threaded executor when spinning the node! 12 | 13 | 14 | class SingleGoalActionServer(SingleGoalMultipleActionServers): 15 | """An action server that only allows a single Action to be executing at one time. 16 | 17 | If a new Action.Goal is received, the existing Action (if there is one) is preemptively canceled. 18 | """ 19 | 20 | def __init__( 21 | self, 22 | node: Node, 23 | action_type: ActionType, 24 | action_topic: str, 25 | execute_callback: Callable, 26 | callback_group: Optional[CallbackGroup] = None, 27 | ) -> None: 28 | super().__init__( 29 | node=node, 30 | action_server_parameters=[(action_type, action_topic, execute_callback, callback_group)], 31 | ) 32 | -------------------------------------------------------------------------------- /synchros2/synchros2/single_goal_multiple_action_servers.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import threading 3 | from typing import Any, Callable, List, Optional, Tuple 4 | 5 | from rclpy.action import ActionServer, CancelResponse, GoalResponse 6 | from rclpy.action.server import GoalEvent, RCLError, ServerGoalHandle 7 | from rclpy.callback_groups import CallbackGroup 8 | from rclpy.impl.rcutils_logger import RcutilsLogger 9 | from rclpy.node import Node 10 | 11 | from synchros2.type_hints import ActionType 12 | from synchros2.utilities import synchronized 13 | 14 | 15 | class SingleGoalMultipleActionServers: 16 | """Wrapper around multiple action servers that only allows a single Action to be executing at one time. 17 | 18 | If a new Action.Goal is received by any of the action servers, the existing Action (if there is one) is preemptively 19 | canceled. 20 | """ 21 | 22 | def __init__( 23 | self, 24 | node: Node, 25 | action_server_parameters: List[Tuple[ActionType, str, Callable, Optional[CallbackGroup]]], 26 | nosync: bool = False, 27 | ) -> None: 28 | """Constructor. 29 | 30 | Args: 31 | node: ROS 2 node to use for action servers. 32 | action_server_parameters: tuples per action server, listing action type, action name, 33 | action execution callback, and action callback group (which may be None). 34 | nosync: whether to synchronize action execution callbacks using locks or not. 35 | Set to True when action execution callback already enforce mutual exclusion. 36 | """ 37 | self._node = node 38 | self._goal_handle_lock = threading.Lock() 39 | self._goal_handle: Optional[ServerGoalHandle] = None 40 | if not nosync: 41 | execution_lock = threading.Lock() 42 | action_server_parameters = [ 43 | (action_type, action_topic, synchronized(execute_callback, execution_lock), callback_group) 44 | for action_type, action_topic, execute_callback, callback_group in action_server_parameters 45 | ] 46 | self._action_servers = [ 47 | ActionServer( 48 | node, 49 | action_type, 50 | action_topic, 51 | execute_callback=execute_callback, 52 | goal_callback=self.goal_callback, 53 | handle_accepted_callback=self.handle_accepted_callback, 54 | cancel_callback=self.cancel_callback, 55 | callback_group=callback_group, 56 | ) 57 | for action_type, action_topic, execute_callback, callback_group in action_server_parameters 58 | ] 59 | 60 | def get_logger(self) -> RcutilsLogger: 61 | """Returns the ros logger""" 62 | return self._node.get_logger() 63 | 64 | def destroy(self) -> None: 65 | """Destroy all of the internal action servers""" 66 | for action_server in self._action_servers: 67 | action_server.destroy() 68 | 69 | def goal_callback(self, goal: Any) -> GoalResponse: 70 | """Accept or reject a client request to begin an action.""" 71 | self.get_logger().info("Received goal request") 72 | return GoalResponse.ACCEPT 73 | 74 | def handle_accepted_callback(self, goal_handle: ServerGoalHandle) -> None: 75 | """Callback triggered when an action is accepted.""" 76 | with self._goal_handle_lock: 77 | # This server only allows one goal at a time 78 | if self._goal_handle is not None: 79 | self.get_logger().info("Canceling previous goal") 80 | try: 81 | self._goal_handle._update_state(GoalEvent.CANCEL_GOAL) 82 | except RCLError as ex: 83 | self.get_logger().debug(f"Failed to cancel goal: {ex}") 84 | self._goal_handle = goal_handle 85 | 86 | goal_handle.execute() 87 | 88 | def cancel_callback(self, cancel_request: Any) -> CancelResponse: 89 | """Accept or reject a client's request to cancel an action.""" 90 | self.get_logger().info("Received cancel request") 91 | return CancelResponse.ACCEPT 92 | -------------------------------------------------------------------------------- /synchros2/synchros2/static_transform_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from typing import Dict, Iterable, Optional, Union, cast 3 | 4 | from geometry_msgs.msg import TransformStamped 5 | from rclpy.node import Node 6 | from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile 7 | from tf2_msgs.msg import TFMessage 8 | 9 | 10 | class StaticTransformBroadcaster: 11 | """A modified :class:`tf2_ros.StaticTransformBroadcaster` that stores transforms sent through it. 12 | 13 | This matches ``rclcpp::StaticTransformBroadcaster`` behavior. 14 | """ 15 | 16 | DEFAULT_QOS = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST) 17 | 18 | def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> None: 19 | """Constructor. 20 | 21 | :param node: The ROS2 node. 22 | :param qos: A QoSProfile or a history depth to apply to the publisher. 23 | """ 24 | if qos is None: 25 | qos = StaticTransformBroadcaster.DEFAULT_QOS 26 | self._net_transforms: Dict[str, TransformStamped] = {} 27 | self._pub = node.create_publisher(TFMessage, "/tf_static", qos) 28 | 29 | def sendTransform(self, transform: Union[TransformStamped, Iterable[TransformStamped]]) -> None: 30 | """Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster. 31 | 32 | Args: 33 | transform: A transform or list of transforms to send. 34 | """ 35 | try: 36 | transforms = list(transform) 37 | except TypeError: # in which case, transform is not iterable 38 | transforms = [cast(TransformStamped, transform)] 39 | self._net_transforms.update({tf.child_frame_id: tf for tf in transforms}) 40 | self._pub.publish(TFMessage(transforms=list(self._net_transforms.values()))) 41 | -------------------------------------------------------------------------------- /synchros2/synchros2/time.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from datetime import datetime, timedelta 4 | from typing import Union 5 | 6 | from rclpy.duration import Duration 7 | from rclpy.time import Time 8 | 9 | 10 | def as_proper_time(time: Union[int, float, datetime, Time]) -> Time: 11 | """Convert `time` to a proper, standardized `Time` object. 12 | 13 | For conversion, the following rules apply: 14 | - if an `int` or a `float` is provided, it is assumed to be a timestamp expressed in seconds. 15 | - if a `datetime` is provided, its UTC `datetime.timestamp()` is used. 16 | - if a `Duration` is provided, it is returned as is. 17 | 18 | Args: 19 | time: the time to be converted. 20 | 21 | Returns: 22 | an standardized `Time` object in ROS time, representing the given `time`. 23 | 24 | Raises: 25 | ValueError: if the given `time` is not of a supported type. 26 | """ 27 | if isinstance(time, (int, float)): 28 | return Time(seconds=time) 29 | if isinstance(time, datetime): 30 | return Time(seconds=time.timestamp()) 31 | if not isinstance(time, Time): 32 | raise ValueError(f"unsupported time type: {time}") 33 | return time 34 | 35 | 36 | def as_proper_duration(duration: Union[int, float, timedelta, Duration]) -> Duration: 37 | """Convert `duration` to a proper, standardized `Duration` object. 38 | 39 | For conversion, the following rules apply: 40 | - if an `int` or a `float` is provided, it is assumed to be a duration expressed in seconds. 41 | - if a `timedelta` is provided, `timedelta.total_seconds()` are used. 42 | - if a `Duration` is provided, it is returned as is. 43 | 44 | Args: 45 | duration: the duration to be converted. 46 | 47 | Returns: 48 | an standardized `Duration` object representing the given `duration`. 49 | 50 | Raises: 51 | ValueError: if the given `duration` is not of a supported type. 52 | """ 53 | if isinstance(duration, (int, float)): 54 | return Duration(seconds=duration) 55 | if isinstance(duration, timedelta): 56 | return Duration(seconds=duration.total_seconds()) 57 | if not isinstance(duration, Duration): 58 | raise ValueError(f"unsupported duration type: {duration}") 59 | return duration 60 | -------------------------------------------------------------------------------- /synchros2/synchros2/type_hints.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from typing import TypeVar 3 | 4 | # The specific Action is created by the .action file 5 | Action = TypeVar("Action") 6 | Action_Goal = TypeVar("Action_Goal") 7 | Action_Feedback = TypeVar("Action_Feedback") 8 | Action_Result = TypeVar("Action_Result") 9 | 10 | # Use for type hints where you are passing the type of an action, not an action object 11 | ActionType = TypeVar("ActionType") 12 | 13 | # The actual code generated for actions does something like this 14 | Action.Goal = Action_Goal # type: ignore 15 | Action.Feedback = Action_Feedback # type: ignore 16 | Action.Result = Action_Result # type: ignore 17 | 18 | # The specific Msg is created by the .msg file 19 | Msg = TypeVar("Msg") 20 | 21 | # The specific Service is created by the .srv file 22 | Srv = TypeVar("Srv") 23 | SrvTypeRequest = TypeVar("SrvTypeRequest") 24 | SrvTypeResponse = TypeVar("SrvTypeResponse") 25 | -------------------------------------------------------------------------------- /synchros2/test/conftest.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from typing import Iterable 4 | 5 | import domain_coordinator 6 | import pytest 7 | 8 | import synchros2.scope as scope 9 | from synchros2.scope import ROSAwareScope 10 | 11 | 12 | @pytest.fixture 13 | def domain_id() -> Iterable[int]: 14 | with domain_coordinator.domain_id() as domain_id: # to ensure node isolation 15 | yield domain_id 16 | 17 | 18 | @pytest.fixture 19 | def ros(domain_id: int) -> Iterable[ROSAwareScope]: 20 | with scope.top(global_=True, domain_id=domain_id, namespace="fixture") as top: 21 | yield top 22 | 23 | 24 | @pytest.fixture 25 | def verbose_ros(domain_id: int) -> Iterable[ROSAwareScope]: 26 | args = ["--ros-args", "--enable-rosout-logs", "--log-level", "INFO"] 27 | with scope.top(args, global_=True, domain_id=domain_id, namespace="fixture") as top: 28 | yield top 29 | -------------------------------------------------------------------------------- /synchros2/test/launch/test_actions.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | from enum import Enum 3 | 4 | import pytest 5 | 6 | from synchros2.launch.actions import DeclareBooleanLaunchArgument, DeclareEnumLaunchArgument 7 | 8 | 9 | def test_declare_boolean_launch_argument_default_value_false() -> None: 10 | arg = DeclareBooleanLaunchArgument("arg", default_value="false") 11 | assert arg.default_value is not None 12 | assert len(arg.default_value) == 1 13 | assert arg.default_value[0].text == "false" 14 | 15 | 16 | def test_declare_boolean_launch_argument_default_value_False() -> None: 17 | arg = DeclareBooleanLaunchArgument("arg", default_value="False") 18 | assert arg.default_value is not None 19 | assert len(arg.default_value) == 1 20 | assert arg.default_value[0].text == "False" 21 | 22 | 23 | def test_declare_boolean_launch_argument_default_value_bool_false() -> None: 24 | arg = DeclareBooleanLaunchArgument("arg", default_value=False) 25 | assert arg.default_value is not None 26 | assert len(arg.default_value) == 1 27 | assert arg.default_value[0].text == "false" 28 | 29 | 30 | def test_declare_boolean_launch_argument_default_value_true() -> None: 31 | arg = DeclareBooleanLaunchArgument("arg", default_value="true") 32 | assert arg.default_value is not None 33 | assert len(arg.default_value) == 1 34 | assert arg.default_value[0].text == "true" 35 | 36 | 37 | def test_declare_boolean_launch_argument_default_value_True() -> None: 38 | arg = DeclareBooleanLaunchArgument("arg", default_value="True") 39 | assert arg.default_value is not None 40 | assert len(arg.default_value) == 1 41 | assert arg.default_value[0].text == "True" 42 | 43 | 44 | def test_declare_boolean_launch_argument_default_value_bool_true() -> None: 45 | arg = DeclareBooleanLaunchArgument("arg", default_value=True) 46 | assert arg.default_value is not None 47 | assert len(arg.default_value) == 1 48 | assert arg.default_value[0].text == "true" 49 | 50 | 51 | def test_declare_boolean_launch_argument_set_choices() -> None: 52 | with pytest.raises(KeyError): 53 | _ = DeclareBooleanLaunchArgument("arg", choices=["some", "choices"]) 54 | 55 | 56 | def test_declare_boolean_launch_argument_default_value_invalid() -> None: 57 | with pytest.raises(ValueError): 58 | _ = DeclareBooleanLaunchArgument("arg", default_value="not true or false") 59 | 60 | 61 | class MockStrEnum(str, Enum): 62 | A = "A" 63 | B = "B" 64 | C = "C" 65 | 66 | 67 | def test_declare_enum_launch_argument_str_enum_str_default_value() -> None: 68 | arg = DeclareEnumLaunchArgument(MockStrEnum, "arg", default_value="A") 69 | assert arg.default_value is not None 70 | assert len(arg.default_value) == 1 71 | assert arg.default_value[0].text == "A" 72 | 73 | 74 | def test_declare_enum_launch_argument_str_enum_enum_default_value() -> None: 75 | arg = DeclareEnumLaunchArgument(MockStrEnum, "arg", default_value=MockStrEnum.A) 76 | assert arg.default_value is not None 77 | assert len(arg.default_value) == 1 78 | assert arg.default_value[0].text == "A" 79 | 80 | 81 | def test_declare_enum_launch_argument_set_choices() -> None: 82 | with pytest.raises(KeyError): 83 | _ = DeclareEnumLaunchArgument(MockStrEnum, "arg", choices=["some", "choices"]) 84 | 85 | 86 | def test_declare_enum_launch_argument_invalid_default() -> None: 87 | with pytest.raises(ValueError): 88 | _ = DeclareEnumLaunchArgument(MockStrEnum, "arg", default_value="D") 89 | -------------------------------------------------------------------------------- /synchros2/test/test_action_client.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import array 3 | import time 4 | from typing import Any 5 | 6 | from example_interfaces.action import Fibonacci 7 | from rclpy.action.server import ActionServer, GoalResponse, ServerGoalHandle 8 | 9 | from synchros2.action_client import ActionClientWrapper 10 | from synchros2.node import Node 11 | from synchros2.scope import ROSAwareScope 12 | 13 | 14 | class FibonacciActionServer(ActionServer): 15 | """Action server to used for testing mostly pulled from ROS2 Action Server tutorial 16 | 17 | Some changes made to allow special testing of timeouts and goal rejections 18 | """ 19 | 20 | def __init__(self, node: Node, name: str, **kwargs: Any) -> None: 21 | super().__init__(node, Fibonacci, name, self.execute_callback, **kwargs) 22 | self._logger = node.get_logger() 23 | 24 | def execute_callback(self, goal_handle: ServerGoalHandle) -> Fibonacci.Result: 25 | self._logger.info("Executing goal...") 26 | # sleep to test timeout 27 | time.sleep(2) 28 | 29 | sequence = [0, 1] 30 | 31 | for i in range(1, goal_handle.request.order): 32 | sequence.append(sequence[i] + sequence[i - 1]) 33 | 34 | goal_handle.succeed() 35 | 36 | result = Fibonacci.Result() 37 | result.sequence = sequence 38 | return result 39 | 40 | 41 | def test_send_goal_and_wait(ros: ROSAwareScope) -> None: 42 | """Test standard operation of send_goal_and_wait""" 43 | assert ros.node is not None 44 | FibonacciActionServer(ros.node, "fibonacci") 45 | action_client = ActionClientWrapper(Fibonacci, "fibonacci", ros.node) 46 | 47 | goal = Fibonacci.Goal() 48 | goal.order = 5 49 | result = action_client.send_goal_and_wait("test_send_goal_and_wait", goal=goal, timeout_sec=5) 50 | assert result is not None 51 | expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) 52 | assert result.sequence == expected_result 53 | 54 | 55 | def test_timeout_send_goal_wait(ros: ROSAwareScope) -> None: 56 | """Test out the timeout of the send_goal_and_wait""" 57 | assert ros.node is not None 58 | FibonacciActionServer(ros.node, "fibonacci") 59 | action_client = ActionClientWrapper(Fibonacci, "fibonacci", ros.node) 60 | 61 | goal = Fibonacci.Goal() 62 | goal.order = 5 63 | result = action_client.send_goal_and_wait("test_timeout_send_goal_wait", goal=goal, timeout_sec=0.5) 64 | assert result is None 65 | 66 | 67 | def test_goal_not_accepted(ros: ROSAwareScope) -> None: 68 | """Test for the goal not accepted pathway should return None""" 69 | 70 | def do_not_accept_goal(goal_request: Fibonacci.Goal) -> GoalResponse: 71 | """Helper callback function for rejecting goals to help test""" 72 | return GoalResponse.REJECT 73 | 74 | assert ros.node is not None 75 | FibonacciActionServer(ros.node, "fibonacci", goal_callback=do_not_accept_goal) 76 | action_client = ActionClientWrapper(Fibonacci, "fibonacci", ros.node) 77 | 78 | goal = Fibonacci.Goal() 79 | goal.order = 5 80 | result = action_client.send_goal_and_wait("test_goal_not_accepted", goal=goal, timeout_sec=5) 81 | assert result is None 82 | -------------------------------------------------------------------------------- /synchros2/test/test_callables.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from collections.abc import Mapping 4 | from typing import Any, Optional 5 | 6 | import pytest 7 | 8 | from synchros2.callables import GeneralizedGuard, generalized_method 9 | from synchros2.futures import wait_for_future 10 | from synchros2.scope import ROSAwareScope 11 | 12 | 13 | class Bucket: 14 | def __init__( 15 | self, 16 | ros: ROSAwareScope, 17 | storage: Optional[Mapping] = None, 18 | ) -> None: 19 | self.executor = ros.executor 20 | if storage is None: 21 | storage = {} 22 | self._storage = dict(storage) 23 | 24 | @generalized_method(transitional=True) 25 | def create(self, content: Any) -> str: 26 | name = str(hash(content)) 27 | if name in self._storage: 28 | raise RuntimeError() 29 | self._storage[name] = content 30 | return name 31 | 32 | @create.sync_overload 33 | def _create_sync(self, name: str, content: Any) -> bool: 34 | if name in self._storage: 35 | return False 36 | self._storage[name] = content 37 | return True 38 | 39 | @create.async_overload 40 | async def _create_async(self, name: str, content: Any) -> bool: 41 | return self._create_sync(name, content) 42 | 43 | @generalized_method 44 | def read(self, name: str) -> Optional[Any]: 45 | return self._storage.get(name) 46 | 47 | @read.async_overload 48 | async def _read_async(self, name: str) -> Optional[Any]: 49 | return self.read(name) 50 | 51 | @generalized_method 52 | async def update(self, name: str, content: Any) -> bool: 53 | if name not in self._storage: 54 | return False 55 | self._storage[name] = content 56 | return True 57 | 58 | @generalized_method 59 | def delete(self, name: str) -> bool: 60 | if name not in self._storage: 61 | return False 62 | del self._storage[name] 63 | return True 64 | 65 | 66 | def test_transitional_method(ros: ROSAwareScope) -> None: 67 | bucket = Bucket(ros) 68 | name = bucket.create("some data") 69 | assert name in bucket._storage 70 | assert bucket._storage[name] == "some data" 71 | 72 | with pytest.raises(RuntimeError): 73 | bucket.create("some data") 74 | 75 | assert not bucket.create.synchronously(name, "some other data") 76 | assert bucket.create.synchronously("my-data", "some other data") 77 | assert "my-data" in bucket._storage 78 | assert bucket._storage["my-data"] == "some other data" 79 | 80 | future = bucket.create.asynchronously("my-data", "more data") 81 | assert wait_for_future(future, timeout_sec=5.0) 82 | assert future.result() is False 83 | 84 | future = bucket.create.asynchronously("extras", "more data") 85 | assert wait_for_future(future, timeout_sec=5.0) 86 | assert future.result() is True 87 | assert "extras" in bucket._storage 88 | assert bucket._storage["extras"] == "more data" 89 | 90 | 91 | def test_nominal_method(ros: ROSAwareScope) -> None: 92 | bucket = Bucket(ros, {"my-data": "some data"}) 93 | 94 | assert bucket.read("my-data") == "some data" 95 | assert bucket.read.synchronously("my-data") == "some data" 96 | future = bucket.read.asynchronously("my-data") 97 | assert wait_for_future(future, timeout_sec=5.0) 98 | assert future.result() == "some data" 99 | 100 | assert not bucket.read("other-data") 101 | assert not bucket.read.synchronously("other-data") 102 | future = bucket.read.asynchronously("other-data") 103 | assert wait_for_future(future, timeout_sec=5.0) 104 | assert future.result() is None 105 | 106 | 107 | def test_sync_only_method(ros: ROSAwareScope) -> None: 108 | bucket = Bucket( 109 | ros, 110 | { 111 | "my-data": "some data", 112 | "extras": "more data", 113 | "old": "old data", 114 | }, 115 | ) 116 | assert bucket.delete("my-data") 117 | assert "my-data" not in bucket._storage 118 | assert not bucket.delete("my-data") 119 | assert bucket.delete.synchronously("extras") 120 | assert "extras" not in bucket._storage 121 | assert not bucket.delete.synchronously("extras") 122 | with pytest.raises(NotImplementedError): 123 | bucket.delete.asynchronously("old") 124 | assert "old" in bucket._storage 125 | 126 | 127 | def test_async_only_method(ros: ROSAwareScope) -> None: 128 | bucket = Bucket(ros, {"my-data": "some data"}) 129 | future = bucket.update("my-data", "new data") 130 | assert wait_for_future(future, timeout_sec=5.0) 131 | assert future.result() is True 132 | assert bucket._storage["my-data"] == "new data" 133 | 134 | future = bucket.update.asynchronously("my-data", "newer data") 135 | assert wait_for_future(future, timeout_sec=5.0) 136 | assert future.result() is True 137 | assert bucket._storage["my-data"] == "newer data" 138 | 139 | with pytest.raises(NotImplementedError): 140 | bucket.update.synchronously("my-data", "") 141 | assert bucket._storage["my-data"] == "newer data" 142 | 143 | 144 | def test_vectorized_method(ros: ROSAwareScope) -> None: 145 | bucket = Bucket( 146 | ros, 147 | { 148 | "my-data": "some data", 149 | "extras": "more data", 150 | }, 151 | ) 152 | data = bucket.read.vectorized(["my-data", "extras"]) 153 | assert data == ["some data", "more data"] 154 | 155 | data = bucket.read.vectorized.synchronously(["my-data", "extras"]) 156 | assert data == ["some data", "more data"] 157 | 158 | future = bucket.read.vectorized.asynchronously(["my-data", "extras"]) 159 | assert wait_for_future(future, timeout_sec=5.0) 160 | assert future.result() == ["some data", "more data"] 161 | 162 | 163 | def test_composed_method(ros: ROSAwareScope) -> None: 164 | bucket = Bucket(ros) 165 | Bucket.create.rebind( 166 | bucket, 167 | bucket.create.compose( 168 | (lambda name, *data: (name, data)), 169 | starred=True, 170 | ), 171 | ) 172 | name = bucket.create("some data") 173 | assert name in bucket._storage 174 | assert bucket._storage[name] == "some data" 175 | 176 | assert bucket.create.synchronously("my-data", "some other data", 1, True) 177 | assert "my-data" in bucket._storage 178 | assert bucket._storage["my-data"] == ("some other data", 1, True) 179 | 180 | future = bucket.create.asynchronously("extras", 0, "more data", False) 181 | assert wait_for_future(future, timeout_sec=5.0) 182 | assert future.result() is True 183 | assert "extras" in bucket._storage 184 | assert bucket._storage["extras"] == (0, "more data", False) 185 | 186 | 187 | def test_guarded_method(ros: ROSAwareScope) -> None: 188 | bucket = Bucket(ros, {"my-data": "some data"}) 189 | 190 | read_permission = False 191 | 192 | def allowed() -> bool: 193 | nonlocal read_permission 194 | return read_permission 195 | 196 | guarded_read = GeneralizedGuard(allowed, bucket.read) 197 | Bucket.read.rebind(bucket, guarded_read) 198 | 199 | with pytest.raises(RuntimeError): 200 | bucket.read("my-data") 201 | 202 | read_permission = True 203 | 204 | assert bucket.read("my-data") == "some data" 205 | -------------------------------------------------------------------------------- /synchros2/test/test_context.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import rclpy 4 | 5 | from synchros2.context import wait_for_shutdown 6 | from synchros2.process import ROSAwareScope 7 | 8 | 9 | def test_wait_for_shutdown(ros: ROSAwareScope) -> None: 10 | """Asserts that wait for shutdown works as expected.""" 11 | assert not wait_for_shutdown(timeout_sec=1.0) 12 | assert ros.executor is not None 13 | ros.executor.create_task(lambda: rclpy.shutdown()) 14 | assert wait_for_shutdown(timeout_sec=10.0) 15 | -------------------------------------------------------------------------------- /synchros2/test/test_executors.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import functools 3 | import threading 4 | import time 5 | from typing import Generator, List 6 | 7 | import pytest 8 | import rclpy 9 | from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup 10 | from rclpy.context import Context 11 | from rclpy.executors import SingleThreadedExecutor 12 | from rclpy.node import Node 13 | from std_srvs.srv import Trigger 14 | 15 | from synchros2.executors import AutoScalingMultiThreadedExecutor, AutoScalingThreadPool, background 16 | from synchros2.futures import wait_for_future 17 | 18 | 19 | @pytest.fixture 20 | def ros_context(domain_id: int) -> Generator[Context, None, None]: 21 | """A fixture yielding a managed rclpy.context.Context instance.""" 22 | context = Context() 23 | rclpy.init(context=context, domain_id=domain_id) 24 | try: 25 | yield context 26 | finally: 27 | context.try_shutdown() 28 | 29 | 30 | @pytest.fixture 31 | def ros_node(ros_context: Context) -> Generator[Node, None, None]: 32 | """A fixture yielding a managed rclpy.node.Node instance.""" 33 | node = rclpy.create_node("pytest_node", context=ros_context) 34 | try: 35 | yield node 36 | finally: 37 | node.destroy_node() 38 | 39 | 40 | def test_autoscaling_thread_pool() -> None: 41 | """Asserts that the autoscaling thread pool scales and de-scales on demand.""" 42 | with AutoScalingThreadPool(max_idle_time=2.0) as pool: 43 | assert len(pool.workers) == 0 44 | assert not bool(pool.working) 45 | 46 | events = [threading.Event() for _ in range(10)] 47 | results = pool.map(lambda i, e: e.wait() and i, range(10), events) 48 | assert len(pool.workers) == len(events) 49 | assert bool(pool.working) 50 | assert not pool.capped 51 | 52 | for e in events: 53 | e.set() 54 | assert list(results) == list(range(10)) 55 | 56 | def predicate() -> bool: 57 | return len(pool.workers) == 0 58 | 59 | with pool.scaling_event: 60 | assert pool.scaling_event.wait_for(predicate, timeout=10) 61 | assert not bool(pool.working) 62 | 63 | 64 | def test_autoscaling_thread_pool_checks_arguments() -> None: 65 | """Asserts that the autoscaling thread pool checks for valid arguments on construction.""" 66 | with pytest.raises(ValueError): 67 | AutoScalingThreadPool(min_workers=-1) 68 | 69 | with pytest.raises(ValueError): 70 | AutoScalingThreadPool(max_workers=0) 71 | 72 | with pytest.raises(ValueError): 73 | AutoScalingThreadPool(max_idle_time=-1) 74 | 75 | with pytest.raises(ValueError): 76 | AutoScalingThreadPool(submission_quota=0) 77 | 78 | with pytest.raises(ValueError): 79 | AutoScalingThreadPool(submission_patience=-1) 80 | 81 | 82 | def test_autoscaling_thread_pool_when_shutdown() -> None: 83 | """Asserts that the autoscaling thread no longer accepts work when shutdown.""" 84 | pool = AutoScalingThreadPool() 85 | 86 | pool.shutdown() 87 | 88 | with pytest.raises(RuntimeError): 89 | pool.submit(lambda: None) 90 | 91 | 92 | def test_autoscaling_thread_pool_with_limits() -> None: 93 | """Asserts that the autoscaling thread pool enforces the user-defined range on the number of workers.""" 94 | with AutoScalingThreadPool(min_workers=2, max_workers=5, max_idle_time=0.1) as pool: 95 | assert len(pool.workers) == 2 96 | assert bool(not pool.working) 97 | 98 | events = [threading.Event() for _ in range(10)] 99 | results = pool.map(lambda i, e: e.wait() and i, range(10), events) 100 | assert len(pool.workers) == 5 101 | assert bool(pool.working) 102 | 103 | for e in events: 104 | e.set() 105 | assert list(results) == list(range(10)) 106 | 107 | def predicate() -> bool: 108 | return len(pool.workers) == 2 109 | 110 | with pool.scaling_event: 111 | assert pool.scaling_event.wait_for(predicate, timeout=10) 112 | assert not pool.working 113 | 114 | 115 | def test_autoscaling_thread_pool_with_quota() -> None: 116 | """Asserts that the autoscaling thread pool respects submission quotas.""" 117 | with AutoScalingThreadPool(submission_quota=5, submission_patience=1.0, max_idle_time=2.0) as pool: 118 | assert len(pool.workers) == 0 119 | assert not bool(pool.working) 120 | 121 | events = [threading.Event() for _ in range(10)] 122 | results = pool.map(lambda i, e: e.wait() and i, range(10), events) 123 | assert len(pool.workers) == 5 124 | assert bool(pool.working) 125 | assert pool.capped 126 | 127 | for e in events[:5]: 128 | e.set() 129 | assert len(pool.workers) == 5 130 | 131 | for e in events[5:]: 132 | e.set() 133 | assert len(pool.workers) == 5 134 | 135 | assert list(results) == list(range(10)) 136 | assert len(pool.workers) == 5 137 | 138 | def predicate() -> bool: 139 | return len(pool.workers) == 0 140 | 141 | with pool.scaling_event: 142 | assert pool.scaling_event.wait_for(predicate, timeout=10) 143 | assert not pool.working 144 | 145 | 146 | def test_autoscaling_executor(ros_context: Context, ros_node: Node) -> None: 147 | """Asserts that the autoscaling multithreaded executor scales to attend a 148 | synchronous service call from a "one-shot" timer callback, serviced by 149 | the same executor. 150 | """ 151 | 152 | def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 153 | response.success = True 154 | return response 155 | 156 | ros_node.create_service( 157 | Trigger, 158 | "/dummy/trigger", 159 | dummy_server_callback, 160 | callback_group=MutuallyExclusiveCallbackGroup(), 161 | ) 162 | 163 | client = ros_node.create_client(Trigger, "/dummy/trigger", callback_group=MutuallyExclusiveCallbackGroup()) 164 | 165 | executor = AutoScalingMultiThreadedExecutor(context=ros_context) 166 | assert len(executor.default_thread_pool.workers) == 0 167 | assert not executor.default_thread_pool.working 168 | executor.add_node(ros_node) 169 | try: 170 | future = executor.create_task(lambda: client.call(Trigger.Request())) 171 | executor.spin_until_future_complete(future, timeout_sec=5) 172 | response = future.result() 173 | assert response.success 174 | assert executor.default_thread_pool.wait(timeout=10) 175 | assert not executor.default_thread_pool.working 176 | finally: 177 | executor.remove_node(ros_node) 178 | executor.shutdown() 179 | 180 | 181 | def test_autoscaling_executor_with_callback_group_affinity(ros_context: Context, ros_node: Node) -> None: 182 | """Asserts that the autoscaling multithreaded executor handles callback group affinity properly""" 183 | 184 | with background(AutoScalingMultiThreadedExecutor(context=ros_context)) as executor: 185 | executor.add_node(ros_node) 186 | 187 | def slow_thread_tracker(threads: List[threading.Thread]) -> None: 188 | threads.append(threading.current_thread()) 189 | time.sleep(0.2) 190 | 191 | timer_period_sec = 0.1 192 | 193 | default_callback_group = ReentrantCallbackGroup() 194 | default_thread_pool_threads: List[threading.Thread] = [] 195 | timer_callback = functools.partial(slow_thread_tracker, default_thread_pool_threads) 196 | ros_node.create_timer(timer_period_sec, timer_callback, default_callback_group) 197 | 198 | callback_group = ReentrantCallbackGroup() 199 | thread_pool = executor.add_static_thread_pool(num_threads=1) 200 | executor.bind(callback_group, thread_pool) 201 | 202 | static_thread_pool_threads: List[threading.Thread] = [] 203 | timer_callback = functools.partial(slow_thread_tracker, static_thread_pool_threads) 204 | ros_node.create_timer(timer_period_sec, timer_callback, callback_group) 205 | 206 | time.sleep(1.0) 207 | 208 | assert len(static_thread_pool_threads) > 0 209 | assert len(default_thread_pool_threads) > len(static_thread_pool_threads) 210 | assert all(thread is static_thread_pool_threads[0] for thread in static_thread_pool_threads[1:]) 211 | assert not any(thread is static_thread_pool_threads[0] for thread in default_thread_pool_threads) 212 | 213 | 214 | def test_background_executor(ros_context: Context) -> None: 215 | """Asserts that an executor can be safely pushed to a background thread.""" 216 | with background(SingleThreadedExecutor(context=ros_context)) as executor: 217 | with pytest.raises(RuntimeError): 218 | executor.spin() 219 | 220 | def deferred() -> bool: 221 | time.sleep(1.0) 222 | return True 223 | 224 | future = executor.create_task(deferred) 225 | 226 | with pytest.raises(RuntimeError): 227 | executor.spin_until_future_complete(future) 228 | 229 | assert wait_for_future(future, timeout_sec=10.0, context=ros_context) 230 | assert future.result() 231 | -------------------------------------------------------------------------------- /synchros2/test/test_feeds.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from typing import Optional, Tuple, cast 4 | 5 | import tf2_ros 6 | from geometry_msgs.msg import ( 7 | Point, 8 | PoseStamped, 9 | TransformStamped, 10 | TwistStamped, 11 | ) 12 | 13 | from synchros2.feeds import ( 14 | AdaptedMessageFeed, 15 | ExactSynchronizedMessageFeed, 16 | FramedMessageFeed, 17 | MessageFeed, 18 | SynchronizedMessageFeed, 19 | ) 20 | from synchros2.filters import Filter 21 | from synchros2.scope import ROSAwareScope 22 | from synchros2.utilities import ensure 23 | 24 | 25 | def test_framed_message_feed(ros: ROSAwareScope) -> None: 26 | tf_buffer = tf2_ros.Buffer() 27 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 28 | framed_message_feed = FramedMessageFeed[Tuple[PoseStamped, TransformStamped]]( 29 | pose_message_feed, 30 | target_frame_id="map", 31 | tf_buffer=tf_buffer, 32 | node=ros.node, 33 | ) 34 | 35 | expected_transform_message = TransformStamped() 36 | expected_transform_message.header.frame_id = "map" 37 | expected_transform_message.child_frame_id = "odom" 38 | expected_transform_message.transform.translation.y = 1.0 39 | expected_transform_message.transform.rotation.w = 1.0 40 | tf_buffer.set_transform_static(expected_transform_message, "pytest") 41 | 42 | expected_pose_message = PoseStamped() 43 | expected_pose_message.header.frame_id = "odom" 44 | expected_pose_message.header.stamp.sec = 1 45 | expected_pose_message.pose.position.x = 1.0 46 | expected_pose_message.pose.orientation.w = 1.0 47 | pose_message_feed.link.signalMessage(expected_pose_message) 48 | 49 | pose_message, transform_message = ensure(framed_message_feed.latest) 50 | assert pose_message.pose.position.x == expected_pose_message.pose.position.x 51 | assert transform_message.transform.translation.y == expected_transform_message.transform.translation.y 52 | 53 | 54 | def test_synchronized_message_feed(ros: ROSAwareScope) -> None: 55 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 56 | twist_message_feed = MessageFeed[TwistStamped](Filter()) 57 | synchronized_message_feed = SynchronizedMessageFeed( 58 | pose_message_feed, 59 | twist_message_feed, 60 | node=ros.node, 61 | ) 62 | 63 | expected_pose_message = PoseStamped() 64 | expected_pose_message.header.frame_id = "odom" 65 | expected_pose_message.header.stamp.sec = 1 66 | expected_pose_message.pose.position.x = 1.0 67 | expected_pose_message.pose.orientation.w = 1.0 68 | pose_message_feed.link.signalMessage(expected_pose_message) 69 | 70 | expected_twist_message = TwistStamped() 71 | expected_twist_message.header.frame_id = "base_link" 72 | expected_twist_message.header.stamp.sec = 1 73 | expected_twist_message.twist.linear.x = 1.0 74 | expected_twist_message.twist.angular.z = 1.0 75 | twist_message_feed.link.signalMessage(expected_twist_message) 76 | 77 | pose_message, twist_message = cast( 78 | Tuple[PoseStamped, TwistStamped], 79 | ensure(synchronized_message_feed.latest), 80 | ) 81 | assert pose_message.pose.position.x == expected_pose_message.pose.position.x 82 | assert twist_message.twist.linear.x == expected_twist_message.twist.linear.x 83 | 84 | 85 | def test_exact_synchronized_message_feed(ros: ROSAwareScope) -> None: 86 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 87 | twist_message_feed = MessageFeed[TwistStamped](Filter()) 88 | synchronized_message_feed = ExactSynchronizedMessageFeed( 89 | pose_message_feed, 90 | twist_message_feed, 91 | node=ros.node, 92 | ) 93 | 94 | expected_pose_message = PoseStamped() 95 | expected_pose_message.header.frame_id = "odom" 96 | expected_pose_message.header.stamp.sec = 1 97 | expected_pose_message.pose.position.x = 1.0 98 | expected_pose_message.pose.orientation.w = 1.0 99 | pose_message_feed.link.signalMessage(expected_pose_message) 100 | 101 | expected_twist_message = TwistStamped() 102 | expected_twist_message.header.frame_id = "base_link" 103 | expected_twist_message.header.stamp.sec = 1 104 | expected_twist_message.twist.linear.x = 1.0 105 | expected_twist_message.twist.angular.z = 1.0 106 | twist_message_feed.link.signalMessage(expected_twist_message) 107 | 108 | pose_message, twist_message = cast( 109 | Tuple[PoseStamped, TwistStamped], 110 | ensure(synchronized_message_feed.latest), 111 | ) 112 | assert pose_message.pose.position.x == expected_pose_message.pose.position.x 113 | assert twist_message.twist.linear.x == expected_twist_message.twist.linear.x 114 | 115 | 116 | def test_adapted_message_feed(ros: ROSAwareScope) -> None: 117 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 118 | position_message_feed = AdaptedMessageFeed[Point]( 119 | pose_message_feed, 120 | fn=lambda message: message.pose.position, 121 | ) 122 | 123 | expected_pose_message = PoseStamped() 124 | expected_pose_message.header.frame_id = "odom" 125 | expected_pose_message.header.stamp.sec = 1 126 | expected_pose_message.pose.position.x = 1.0 127 | expected_pose_message.pose.position.z = -1.0 128 | expected_pose_message.pose.orientation.w = 1.0 129 | pose_message_feed.link.signalMessage(expected_pose_message) 130 | 131 | position_message: Point = ensure(position_message_feed.latest) 132 | # no copies are expected, thus an identity check is valid 133 | assert position_message is expected_pose_message.pose.position 134 | 135 | 136 | def test_masked_message_feed(ros: ROSAwareScope) -> None: 137 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 138 | position_masking_feed = AdaptedMessageFeed[Point]( 139 | pose_message_feed, 140 | fn=lambda message: message if message.pose.position.x > 0.0 else None, 141 | ) 142 | expected_pose_message0 = PoseStamped() 143 | expected_pose_message0.header.frame_id = "odom" 144 | expected_pose_message0.header.stamp.sec = 1 145 | expected_pose_message0.pose.position.x = -1.0 146 | expected_pose_message0.pose.position.z = -1.0 147 | expected_pose_message0.pose.orientation.w = 1.0 148 | pose_message_feed.link.signalMessage(expected_pose_message0) 149 | assert position_masking_feed.latest is None 150 | 151 | expected_pose_message1 = PoseStamped() 152 | expected_pose_message1.header.frame_id = "odom" 153 | expected_pose_message1.header.stamp.sec = 2 154 | expected_pose_message1.pose.position.x = 1.0 155 | expected_pose_message1.pose.position.z = -1.0 156 | expected_pose_message1.pose.orientation.w = 1.0 157 | pose_message_feed.link.signalMessage(expected_pose_message1) 158 | 159 | pose_message: Point = ensure(position_masking_feed.latest) 160 | # no copies are expected, thus an identity check is valid 161 | assert pose_message is expected_pose_message1 162 | 163 | 164 | def test_message_feed_recalls(ros: ROSAwareScope) -> None: 165 | pose_message_feed = MessageFeed[PoseStamped](Filter()) 166 | 167 | latest_message: Optional[PoseStamped] = None 168 | 169 | def callback(message: PoseStamped) -> None: 170 | nonlocal latest_message 171 | latest_message = message 172 | 173 | conn = pose_message_feed.recall(callback) 174 | 175 | first_pose_message = PoseStamped() 176 | first_pose_message.header.stamp.sec = 1 177 | pose_message_feed.link.signalMessage(first_pose_message) 178 | 179 | assert latest_message is not None 180 | assert latest_message.header.stamp.sec == first_pose_message.header.stamp.sec 181 | 182 | conn.close() 183 | 184 | second_pose_message = PoseStamped() 185 | second_pose_message.header.stamp.sec = 2 186 | pose_message_feed.link.signalMessage(second_pose_message) 187 | 188 | assert latest_message.header.stamp.sec != second_pose_message.header.stamp.sec 189 | assert latest_message.header.stamp.sec == first_pose_message.header.stamp.sec 190 | -------------------------------------------------------------------------------- /synchros2/test/test_filters.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from typing import List, Tuple 4 | 5 | import tf2_ros 6 | from geometry_msgs.msg import PoseStamped, TransformStamped 7 | 8 | from synchros2.filters import Filter, TransformFilter 9 | 10 | 11 | def test_transform_wait() -> None: 12 | source = Filter() 13 | tf_buffer = tf2_ros.Buffer() 14 | tf_filter = TransformFilter(source, "map", tf_buffer, tolerance_sec=1.0) 15 | sink: List[Tuple[PoseStamped, TransformStamped]] = [] 16 | tf_filter.registerCallback(lambda *msgs: sink.append((msgs[0], msgs[1]))) 17 | assert len(sink) == 0 18 | pose_message = PoseStamped() 19 | pose_message.header.frame_id = "odom" 20 | pose_message.header.stamp.sec = 1 21 | pose_message.pose.position.x = 1.0 22 | pose_message.pose.orientation.w = 1.0 23 | source.signalMessage(pose_message) 24 | assert len(sink) == 0 25 | transform_message = TransformStamped() 26 | transform_message.header.frame_id = "map" 27 | transform_message.child_frame_id = "odom" 28 | transform_message.transform.rotation.w = 1.0 29 | tf_buffer.set_transform_static(transform_message, "pytest") 30 | assert len(sink) == 1 31 | filtered_pose_message, filtered_transform_message = sink.pop() 32 | assert filtered_pose_message.header.frame_id == pose_message.header.frame_id 33 | assert filtered_pose_message.pose.position.x == pose_message.pose.position.x 34 | assert filtered_pose_message.pose.orientation.w == pose_message.pose.orientation.w 35 | assert filtered_transform_message.header.frame_id == transform_message.header.frame_id 36 | assert filtered_transform_message.child_frame_id == transform_message.child_frame_id 37 | assert filtered_transform_message.transform.rotation.w == transform_message.transform.rotation.w 38 | 39 | 40 | def test_old_transform_filtering() -> None: 41 | source = Filter() 42 | tf_buffer = tf2_ros.Buffer() 43 | tf_filter = TransformFilter(source, "map", tf_buffer, tolerance_sec=2.0) 44 | sink: List[Tuple[PoseStamped, TransformStamped]] = [] 45 | tf_filter.registerCallback(lambda *msgs: sink.append((msgs[0], msgs[1]))) 46 | assert len(sink) == 0 47 | first_pose_message = PoseStamped() 48 | first_pose_message.header.frame_id = "odom" 49 | first_pose_message.pose.position.x = 1.0 50 | first_pose_message.pose.orientation.w = 1.0 51 | source.signalMessage(first_pose_message) 52 | assert len(sink) == 0 53 | second_pose_message = PoseStamped() 54 | second_pose_message.header.frame_id = "odom" 55 | second_pose_message.header.stamp.sec = 10 56 | second_pose_message.pose.position.x = 2.0 57 | second_pose_message.pose.orientation.w = 1.0 58 | source.signalMessage(second_pose_message) 59 | assert len(sink) == 0 60 | transform_message = TransformStamped() 61 | transform_message.header.frame_id = "map" 62 | transform_message.child_frame_id = "odom" 63 | transform_message.transform.rotation.w = 1.0 64 | tf_buffer.set_transform_static(transform_message, "pytest") 65 | assert len(sink) == 1 66 | filtered_pose_message, filtered_transform_message = sink.pop() 67 | assert filtered_pose_message.header.frame_id == second_pose_message.header.frame_id 68 | assert filtered_pose_message.pose.position.x == second_pose_message.pose.position.x 69 | assert filtered_pose_message.pose.orientation.w == second_pose_message.pose.orientation.w 70 | assert filtered_transform_message.header.frame_id == transform_message.header.frame_id 71 | assert filtered_transform_message.child_frame_id == transform_message.child_frame_id 72 | assert filtered_transform_message.transform.rotation.w == transform_message.transform.rotation.w 73 | -------------------------------------------------------------------------------- /synchros2/test/test_futures.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from rclpy.task import Future 4 | 5 | from synchros2.futures import wait_for_future 6 | from synchros2.scope import ROSAwareScope 7 | 8 | 9 | def test_wait_for_cancelled_future(ros: ROSAwareScope) -> None: 10 | """Asserts that waiting for a cancelled future does not hang indefinitely.""" 11 | future = Future() 12 | future.cancel() 13 | 14 | assert not wait_for_future(future, context=ros.context) 15 | assert future.cancelled() 16 | -------------------------------------------------------------------------------- /synchros2/test/test_graph.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import pytest 3 | from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile 4 | from std_msgs.msg import Int8 5 | 6 | from synchros2.graph import ensure_num_publishers, ensure_num_subscriptions 7 | from synchros2.scope import ROSAwareScope 8 | 9 | DEFAULT_QOS_PROFILE = QoSProfile( 10 | durability=DurabilityPolicy.TRANSIENT_LOCAL, 11 | history=HistoryPolicy.KEEP_ALL, 12 | depth=1, 13 | ) 14 | 15 | 16 | def test_ensure_num_publishers(ros: ROSAwareScope) -> None: 17 | """Asserts that checking for publisher matching on a subscription works as expected.""" 18 | assert ros.node is not None 19 | assert ensure_num_publishers(ros.node, "sequence", 0, timeout_sec=5.0) == 0 20 | with pytest.raises(ValueError): 21 | ensure_num_publishers(ros.node, "sequence", 1, timeout_sec=1.0) 22 | ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 23 | assert ensure_num_publishers(ros.node, "sequence", 1, timeout_sec=5.0) == 1 24 | 25 | 26 | def test_ensure_num_subscriptions(ros: ROSAwareScope) -> None: 27 | """Asserts that checking for publisher matching on a subscription works as expected.""" 28 | assert ros.node is not None 29 | assert ensure_num_subscriptions(ros.node, "sequence", 0, timeout_sec=5.0) == 0 30 | with pytest.raises(ValueError): 31 | ensure_num_subscriptions(ros.node, "sequence", 1, timeout_sec=1.0) 32 | ros.node.create_subscription(Int8, "sequence", lambda _: None, DEFAULT_QOS_PROFILE) 33 | assert ensure_num_subscriptions(ros.node, "sequence", 1, timeout_sec=5.0) == 1 34 | -------------------------------------------------------------------------------- /synchros2/test/test_integration.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import math 3 | import random 4 | from typing import Any 5 | 6 | import pytest 7 | import tf2_ros 8 | from example_interfaces.action import Fibonacci 9 | from example_interfaces.srv import AddTwoInts 10 | from geometry_msgs.msg import TransformStamped 11 | from rclpy.action.client import ActionClient 12 | from rclpy.action.server import ActionServer, ServerGoalHandle 13 | from rclpy.duration import Duration 14 | from rclpy.time import Time 15 | 16 | from synchros2.futures import wait_for_future 17 | from synchros2.node import Node 18 | from synchros2.scope import ROSAwareScope 19 | 20 | 21 | class MinimalTransformPublisher(Node): 22 | """A minimal ROS 2 node broadcasting a fixed transform over /tf.""" 23 | 24 | frame_id = "world" 25 | child_frame_id = "robot" 26 | 27 | def __init__(self, node_name: str = "minimal_transform_publisher", **kwargs: Any) -> None: 28 | super().__init__(node_name, **kwargs) 29 | self._broadcaster = tf2_ros.TransformBroadcaster(self) 30 | self._transform = TransformStamped() 31 | self._transform.header.frame_id = self.frame_id 32 | self._transform.child_frame_id = self.child_frame_id 33 | self._transform.transform.translation.x = 1.0 34 | self._transform.transform.translation.y = 2.0 35 | self._transform.transform.translation.z = 3.0 36 | self._transform.transform.rotation.z = math.sin(math.radians(45) / 2) 37 | self._transform.transform.rotation.w = math.cos(math.radians(45) / 2) 38 | 39 | self.callback() # do not wait for first publish 40 | self._timer = self.create_timer(1, self.callback) 41 | 42 | def callback(self) -> None: 43 | self._transform.header.stamp = self.get_clock().now().to_msg() 44 | self._broadcaster.sendTransform(self._transform) 45 | 46 | 47 | class MinimalServer(Node): 48 | """A minimal ROS 2 node serving an example_interfaces.srv.AddTwoInts service.""" 49 | 50 | def __init__(self, node_name: str = "minimal_server", **kwargs: Any) -> None: 51 | super().__init__(node_name, **kwargs) 52 | self._service_server = self.create_service(AddTwoInts, "add_two_ints", self.service_callback) 53 | 54 | def service_callback(self, request: AddTwoInts.Request, response: AddTwoInts.Response) -> AddTwoInts.Response: 55 | response.sum = request.a + request.b 56 | return response 57 | 58 | 59 | class MinimalActionServer(Node): 60 | """A minimal ROS 2 node serving an example_interfaces.action.Fibonacci action.""" 61 | 62 | def __init__(self, node_name: str = "minimal_action_server", **kwargs: Any) -> None: 63 | super().__init__(node_name, **kwargs) 64 | self._action_server = ActionServer(self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback) 65 | 66 | def execute_callback(self, goal_handle: ServerGoalHandle) -> Fibonacci.Result: 67 | sequence = [0, 1] 68 | 69 | for i in range(1, goal_handle.request.order): 70 | sequence.append(sequence[i] + sequence[i - 1]) 71 | feedback = Fibonacci.Feedback() 72 | feedback.sequence = sequence 73 | goal_handle.publish_feedback(feedback) 74 | goal_handle.succeed() 75 | 76 | result = Fibonacci.Result() 77 | result.sequence = sequence 78 | return result 79 | 80 | 81 | @pytest.fixture(autouse=True) 82 | def ros_graph(ros: ROSAwareScope) -> None: 83 | """An automatic fixture managing execution of multiple nodes for testing purposes.""" 84 | ros.load(MinimalTransformPublisher) 85 | ros.load(MinimalServer) 86 | ros.load(MinimalActionServer) 87 | return 88 | 89 | 90 | def test_blocking_sequence(ros: ROSAwareScope) -> None: 91 | """Asserts that a blocking call sequence (single-nested if you follow the execution path 92 | across callbacks) is possible when using a multi-threaded executor and callback isolation. 93 | """ 94 | assert ros.node is not None 95 | tf_buffer = tf2_ros.Buffer() 96 | tf2_ros.TransformListener(tf_buffer, node=ros.node, spin_thread=False) 97 | 98 | client = ros.node.create_client(AddTwoInts, "add_two_ints") 99 | action_client = ActionClient(ros.node, Fibonacci, "compute_fibonacci_sequence") 100 | 101 | def blocking_sequence() -> TransformStamped: 102 | generator = random.Random(0) 103 | seed = generator.randint(5, 10) 104 | assert client.wait_for_service(timeout_sec=5) 105 | response = client.call(AddTwoInts.Request(a=seed, b=3)) 106 | assert response.sum == seed + 3 107 | 108 | assert action_client.wait_for_server(timeout_sec=5) 109 | feedback = [] 110 | result = action_client.send_goal( 111 | Fibonacci.Goal(order=response.sum), 112 | feedback_callback=lambda f: feedback.append(f.feedback), 113 | ).result 114 | assert len(feedback) > 0 115 | partial_sequence = feedback[-1].sequence 116 | assert result.sequence[: len(partial_sequence)] == partial_sequence 117 | 118 | timeout = Duration(seconds=5) 119 | assert tf_buffer.can_transform( 120 | MinimalTransformPublisher.frame_id, 121 | MinimalTransformPublisher.child_frame_id, 122 | Time(), 123 | timeout, 124 | ) 125 | 126 | assert ros.node is not None 127 | time = ros.node.get_clock().now() 128 | time += Duration(seconds=result.sequence[-1] * 10e-3) 129 | return tf_buffer.lookup_transform( 130 | MinimalTransformPublisher.frame_id, 131 | MinimalTransformPublisher.child_frame_id, 132 | time, 133 | timeout, 134 | ) 135 | 136 | assert ros.executor is not None 137 | future = ros.executor.create_task(blocking_sequence) 138 | assert wait_for_future(future, timeout_sec=10) 139 | transform = future.result() 140 | assert transform is not None 141 | assert transform.header.frame_id == MinimalTransformPublisher.frame_id 142 | 143 | 144 | def test_chain_sequence(ros: ROSAwareScope) -> None: 145 | """Asserts that a chained call sequence (double-nested if you follow the execution path 146 | across callbacks) is possible when using a multi-threaded executor and callback isolation. 147 | """ 148 | assert ros.node is not None 149 | action_client = ActionClient(ros.node, Fibonacci, "compute_fibonacci_sequence") 150 | 151 | def add_fibonacci_sequences_server_callback( 152 | request: AddTwoInts.Request, 153 | response: AddTwoInts.Response, 154 | ) -> AddTwoInts.Response: 155 | if not action_client.wait_for_server(timeout_sec=5): 156 | response.sum = -1 157 | return response 158 | result_a = action_client.send_goal(Fibonacci.Goal(order=request.a)).result 159 | result_b = action_client.send_goal(Fibonacci.Goal(order=request.b)).result 160 | response.sum = sum(result_a.sequence) + sum(result_b.sequence) 161 | return response 162 | 163 | ros.node.create_service(AddTwoInts, "add_two_fibonacci_sequences", add_fibonacci_sequences_server_callback) 164 | 165 | client = ros.node.create_client(AddTwoInts, "add_two_fibonacci_sequences") 166 | 167 | def chain_sequence() -> TransformStamped: 168 | assert client.wait_for_service(timeout_sec=5) 169 | response = client.call(AddTwoInts.Request(a=6, b=12)) 170 | return response.sum == 396 171 | 172 | assert ros.executor is not None 173 | future = ros.executor.create_task(chain_sequence) 174 | assert wait_for_future(future, timeout_sec=10) 175 | assert future.result() 176 | -------------------------------------------------------------------------------- /synchros2/test/test_logging.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import logging 3 | import threading 4 | from typing import List 5 | 6 | from rcl_interfaces.msg import Log 7 | from rclpy.clock import ROSClock 8 | from rclpy.time import Time 9 | 10 | from synchros2.futures import unwrap_future 11 | from synchros2.logging import LoggingSeverity, as_memoizing_logger, logs_to_ros 12 | from synchros2.scope import ROSAwareScope 13 | from synchros2.subscription import Subscription 14 | 15 | 16 | def test_memoizing_logger(verbose_ros: ROSAwareScope) -> None: 17 | messages: List[str] = [] 18 | cv = threading.Condition() 19 | 20 | def callback(message: Log) -> None: 21 | nonlocal messages, cv 22 | with cv: 23 | messages.append(message.msg) 24 | cv.notify() 25 | 26 | assert verbose_ros.node is not None 27 | verbose_ros.node.create_subscription(Log, "/rosout", callback, 10) 28 | 29 | logger = as_memoizing_logger(verbose_ros.node.get_logger()) 30 | logger.set_level(LoggingSeverity.INFO) 31 | 32 | assert not logger.debug("Debug message should not be logged") 33 | 34 | for i in range(2): 35 | is_first_iteration = i == 0 36 | did_log_once = logger.error( 37 | f"Error message should have been logged only if {i} == 0", 38 | once=True, 39 | ) 40 | assert did_log_once == is_first_iteration 41 | did_skip_first_log = logger.error( 42 | f"Error message should have been logged only if {i} != 0", 43 | skip_first=True, 44 | ) 45 | assert did_skip_first_log != is_first_iteration 46 | 47 | assert logger.warning("Warning message always logged", once=True) 48 | assert logger.warning("Warning message always logged", once=True) 49 | 50 | fake_clock = ROSClock() 51 | fake_clock._set_ros_time_is_active(True) 52 | 53 | num_throttled_logs = 2 54 | num_attempts_per_sec = 5 55 | for i in range(num_attempts_per_sec * num_throttled_logs): 56 | fake_clock.set_ros_time_override(Time(seconds=float(i) / num_attempts_per_sec)) 57 | assert logger.info( 58 | "Info message should be throttled", 59 | throttle_duration_sec=1.0, 60 | throttle_time_source_type=fake_clock, 61 | ) == (i % num_attempts_per_sec == 0) 62 | 63 | expected_messages = [ 64 | "Error message should have been logged only if 0 == 0", 65 | "Error message should have been logged only if 1 != 0", 66 | "Warning message always logged", 67 | "Warning message always logged", 68 | ] + ["Info message should be throttled"] * num_throttled_logs 69 | 70 | def all_messages_arrived() -> bool: 71 | return len(messages) == len(expected_messages) 72 | 73 | with cv: 74 | assert cv.wait_for(all_messages_arrived, timeout=5.0) 75 | assert messages == expected_messages 76 | 77 | 78 | def test_log_forwarding(verbose_ros: ROSAwareScope) -> None: 79 | assert verbose_ros.node is not None 80 | rosout = Subscription(Log, "/rosout", 10, node=verbose_ros.node) 81 | assert unwrap_future(rosout.publisher_matches(1), timeout_sec=5.0) > 0 82 | 83 | with logs_to_ros(verbose_ros.node): 84 | logger = logging.getLogger("my_logger") 85 | logger.setLevel(logging.INFO) 86 | logger.propagate = True # ensure propagation is enabled 87 | logger.info("test") 88 | 89 | log = unwrap_future(rosout.update, timeout_sec=5.0) 90 | # NOTE(hidmic) why are log levels of bytestring type !? 91 | assert log.level == int.from_bytes(Log.INFO, byteorder="little") 92 | assert log.name == verbose_ros.node.get_logger().name 93 | assert log.msg == "(logging.my_logger) test" 94 | -------------------------------------------------------------------------------- /synchros2/test/test_node.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import threading 4 | from typing import Any, Generator 5 | 6 | import pytest 7 | import rclpy 8 | from rcl_interfaces.srv import GetParameters 9 | from rclpy.context import Context 10 | from std_srvs.srv import Trigger 11 | 12 | from synchros2.executors import AutoScalingMultiThreadedExecutor 13 | from synchros2.node import Node 14 | from synchros2.scope import ROSAwareScope 15 | 16 | 17 | @pytest.fixture 18 | def ros_context(domain_id: int) -> Generator[Context, None, None]: 19 | """A fixture yielding a managed rclpy.context.Context instance.""" 20 | context = Context() 21 | rclpy.init(context=context, domain_id=domain_id) 22 | try: 23 | yield context 24 | finally: 25 | context.try_shutdown() 26 | 27 | 28 | def test_node_destruction_during_execution(ros_context: Context) -> None: 29 | """Asserts that node destructionthe autoscaling multithreaded executor scales to attend a 30 | synchronous service call from a "one-shot" timer callback, serviced by 31 | the same executor. 32 | """ 33 | 34 | def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 35 | response.success = True 36 | return response 37 | 38 | node = Node("pytest_node", context=ros_context) 39 | node.create_service(Trigger, "/dummy/trigger", dummy_server_callback) 40 | client = node.create_client(Trigger, "/dummy/trigger") 41 | 42 | executor = AutoScalingMultiThreadedExecutor(max_threads=1, context=ros_context) 43 | executor.add_node(node) 44 | 45 | barrier = threading.Barrier(2) 46 | try: 47 | # First smoke test the executor with a service invocation 48 | future = client.call_async(Trigger.Request()) 49 | executor.spin_until_future_complete(future, timeout_sec=5.0) 50 | assert future.done() and future.result().success 51 | # Then block its sole worker thread 52 | executor.create_task(lambda: barrier.wait()) 53 | executor.spin_once() 54 | # Then queue node destruction 55 | executor.create_task(lambda: node.destroy_node()) 56 | executor.spin_once() 57 | assert not bool(node.destruction_requested) # still queued 58 | # Then queue another service invocation 59 | future = client.call_async(Trigger.Request()) 60 | executor.spin_once() 61 | # Unblock worker thread in executor 62 | barrier.wait() 63 | # Check that executor wraps up early due to node destruction 64 | executor.spin_until_future_complete(future, timeout_sec=5.0) 65 | assert node.destruction_requested 66 | assert executor.default_thread_pool.wait(timeout=5.0) 67 | assert not future.done() # future response will never be resolved 68 | finally: 69 | barrier.reset() 70 | executor.remove_node(node) 71 | executor.shutdown() 72 | 73 | 74 | def test_node_post_initialization(ros: ROSAwareScope) -> None: 75 | """Asserts that Node post initialization is honored and that it affords blocking calls.""" 76 | 77 | class ComplexProxyNode(Node): 78 | def __init__(self, *args: Any, remote_node_name: str, **kwargs: Any) -> None: 79 | super().__init__(f"proxy_node_for_{remote_node_name}", *args, **kwargs) 80 | self._get_remote_node_parameters_client = self.create_client( 81 | GetParameters, 82 | f"{remote_node_name}/get_parameters", 83 | ) 84 | 85 | def __post_init__(self) -> None: 86 | response = self._get_remote_node_parameters_client.call( 87 | GetParameters.Request(names=["verbose"]), 88 | ) 89 | self.verbose = response.values[0].bool_value 90 | 91 | assert ros.node is not None 92 | ros.node.declare_parameter("verbose", True) 93 | with ros.managed(ComplexProxyNode, remote_node_name=ros.node.get_name()) as node: 94 | assert node.verbose 95 | -------------------------------------------------------------------------------- /synchros2/test/test_process.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import argparse 3 | import threading 4 | import unittest.mock as mock 5 | 6 | import pytest 7 | from geometry_msgs.msg import TransformStamped 8 | from rclpy.task import Future 9 | from std_srvs.srv import Trigger 10 | 11 | import synchros2.process as process 12 | from synchros2.futures import wait_for_future 13 | from synchros2.static_transform_broadcaster import StaticTransformBroadcaster 14 | 15 | 16 | def test_process_wrapping() -> None: 17 | """Asserts that the process bound node is made available.""" 18 | 19 | @process.main() 20 | def main() -> int: 21 | def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 22 | response.success = True 23 | return response 24 | 25 | assert main.node is not None 26 | main.node.create_service(Trigger, "/dummy/trigger", dummy_server_callback) 27 | 28 | client = main.node.create_client(Trigger, "/dummy/trigger") 29 | assert client.wait_for_service(timeout_sec=10) 30 | 31 | response = client.call(Trigger.Request()) 32 | assert response.success 33 | return 0 34 | 35 | assert main() == 0 36 | 37 | 38 | def test_process_exception_propagates() -> None: 39 | """Asserts that any exception raised within the main thread propagates.""" 40 | 41 | barrier = threading.Barrier(2) 42 | 43 | def wait_forever() -> None: 44 | barrier.wait() 45 | wait_for_future(Future()) 46 | 47 | @process.main() 48 | def main() -> int: 49 | assert main.executor is not None 50 | main.executor.create_task(wait_forever) 51 | barrier.wait() 52 | raise RuntimeError("probe") 53 | 54 | with pytest.raises(RuntimeError, match="probe"): 55 | main() 56 | 57 | 58 | def test_process_using_tf() -> None: 59 | """Asserts that the process bound tf listener is made available.""" 60 | 61 | @process.main(uses_tf=True) 62 | def main() -> int: 63 | assert main.node is not None 64 | stamp = main.node.get_clock().now().to_msg() 65 | 66 | expected_transform = TransformStamped() 67 | expected_transform.header.stamp = stamp 68 | expected_transform.header.frame_id = "world" 69 | expected_transform.child_frame_id = "robot" 70 | expected_transform.transform.rotation.w = 1.0 71 | tf_broadcaster = StaticTransformBroadcaster(main.node) 72 | tf_broadcaster.sendTransform(expected_transform) 73 | 74 | assert main.tf_listener is not None 75 | transform = main.tf_listener.lookup_a_tform_b("world", "robot", stamp, timeout_sec=5.0, wait_for_frames=True) 76 | assert transform.header.stamp == expected_transform.header.stamp 77 | assert transform.transform.rotation.w == expected_transform.transform.rotation.w 78 | return 0 79 | 80 | assert main() == 0 81 | 82 | 83 | def test_command_wrapping() -> None: 84 | """Asserts that the process bound node is made available.""" 85 | 86 | def cli() -> argparse.ArgumentParser: 87 | parser = argparse.ArgumentParser("test_command") 88 | parser.add_argument("robot") 89 | return parser 90 | 91 | @process.main(cli()) 92 | def main(args: argparse.Namespace) -> int: 93 | assert main.node is not None 94 | assert main.node.get_fully_qualified_name() == "/test_command" 95 | assert args.robot == "spot" 96 | return 0 97 | 98 | assert main(["test_command", "spot"]) == 0 99 | 100 | 101 | def test_cli_configuration() -> None: # type: ignore 102 | """Asserts that CLI can affect process configuration.""" 103 | 104 | def cli() -> argparse.ArgumentParser: 105 | parser = argparse.ArgumentParser("test_command") 106 | parser.add_argument("robot") 107 | parser.add_argument("-q", "--quiet", action="store_true") 108 | parser.set_defaults(process_args=lambda args: dict(forward_logging=not args.quiet)) 109 | return parser 110 | 111 | @process.main(cli(), namespace="/{robot}") 112 | def main(args: argparse.Namespace) -> int: 113 | assert main.node is not None 114 | assert main.node.get_fully_qualified_name() == "/spot/test_command" 115 | assert args.robot == "spot" 116 | return 0 117 | 118 | with mock.patch("synchros2.scope.logs_to_ros") as logs_to_ros: 119 | assert main(["test_command", "spot", "--quiet"]) == 0 120 | assert not logs_to_ros.called 121 | -------------------------------------------------------------------------------- /synchros2/test/test_publisher.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Boston Dynamics AI Institute Inc. All rights reserved. 2 | 3 | import std_msgs.msg 4 | from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile 5 | 6 | from synchros2.futures import wait_for_future 7 | from synchros2.publisher import Publisher 8 | from synchros2.scope import ROSAwareScope 9 | 10 | DEFAULT_QOS_PROFILE = QoSProfile( 11 | durability=DurabilityPolicy.TRANSIENT_LOCAL, 12 | history=HistoryPolicy.KEEP_ALL, 13 | depth=1, 14 | ) 15 | 16 | 17 | def test_publisher_matching_subscriptions(ros: ROSAwareScope) -> None: 18 | """Asserts that checking for subscription matching on a publisher works as expected.""" 19 | assert ros.node is not None 20 | sequence = Publisher( # type: ignore[var-annotated] 21 | std_msgs.msg.Int8, 22 | "sequence", 23 | qos_profile=DEFAULT_QOS_PROFILE, 24 | node=ros.node, 25 | ) 26 | assert sequence.matched_subscriptions == 0 27 | future = sequence.subscription_matches(1) 28 | assert not future.done() 29 | future.cancel() 30 | 31 | ros.node.create_subscription( 32 | std_msgs.msg.Int8, 33 | "sequence", 34 | qos_profile=DEFAULT_QOS_PROFILE, 35 | callback=lambda msg: None, 36 | ) 37 | assert wait_for_future(sequence.subscription_matches(1), timeout_sec=5.0) # type: ignore[arg-type] 38 | assert sequence.matched_subscriptions == 1 39 | -------------------------------------------------------------------------------- /synchros2/test/test_service.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import pytest 4 | from rclpy.duration import Duration 5 | from std_srvs.srv import Trigger 6 | 7 | from synchros2.futures import wait_for_future 8 | from synchros2.scope import ROSAwareScope 9 | from synchros2.service import Serviced, ServiceError, ServiceTimeout 10 | 11 | 12 | def succeeding_callback(request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 13 | response.success = True 14 | return response 15 | 16 | 17 | def test_successful_synchronous_service_invocation(ros: ROSAwareScope) -> None: 18 | assert ros.node is not None 19 | ros.node.create_service(Trigger, "trigger_something", succeeding_callback) 20 | trigger_something: Serviced[Trigger.Request, Trigger.Response] = Serviced(Trigger, "trigger_something") 21 | response = trigger_something(timeout_sec=5.0) 22 | assert response.success 23 | 24 | 25 | def test_successful_asynchronous_service_invocation(ros: ROSAwareScope) -> None: 26 | assert ros.node is not None 27 | ros.node.create_service(Trigger, "trigger_something", succeeding_callback) 28 | trigger_something: Serviced[Trigger.Request, Trigger.Response] = Serviced(Trigger, "trigger_something") 29 | service = trigger_something.asynchronously() 30 | assert wait_for_future(service, timeout_sec=5.0) 31 | response = service.result() 32 | assert response.success 33 | 34 | 35 | def test_timed_out_synchronous_service_invocation(ros: ROSAwareScope) -> None: 36 | assert ros.node is not None 37 | clock = ros.node.get_clock() 38 | 39 | def callback(request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 40 | clock.sleep_until(clock.now() + Duration(seconds=5.0)) 41 | response.success = True 42 | return response 43 | 44 | ros.node.create_service(Trigger, "trigger_something", callback) 45 | trigger_something: Serviced[Trigger.Request, Trigger.Response] = Serviced(Trigger, "trigger_something") 46 | with pytest.raises(ServiceTimeout): 47 | trigger_something(timeout_sec=0.01) 48 | 49 | 50 | def failing_callback(request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: 51 | response.success = False 52 | return response 53 | 54 | 55 | def test_failing_synchronous_service_invocation(ros: ROSAwareScope) -> None: 56 | assert ros.node is not None 57 | ros.node.create_service(Trigger, "trigger_something", failing_callback) 58 | trigger_something: Serviced[Trigger.Request, Trigger.Response] = Serviced(Trigger, "trigger_something") 59 | response = trigger_something(nothrow=True, timeout_sec=5.0) 60 | assert not response.success 61 | with pytest.raises(ServiceError) as exc: 62 | trigger_something(timeout_sec=5.0) 63 | response = exc.value.service.result() 64 | assert not response.success 65 | 66 | 67 | def test_failing_asynchronous_service_invocation(ros: ROSAwareScope) -> None: 68 | assert ros.node is not None 69 | ros.node.create_service(Trigger, "trigger_something", failing_callback) 70 | trigger_something: Serviced[Trigger.Request, Trigger.Response] = Serviced(Trigger, "trigger_something") 71 | service = trigger_something.asynchronously() 72 | assert wait_for_future(service, timeout_sec=5.0) 73 | response = service.result() 74 | assert not response.success 75 | -------------------------------------------------------------------------------- /synchros2/test/test_service_handle.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import inspect 3 | from typing import Any, Optional, Tuple, Type 4 | 5 | from rclpy.client import Client 6 | from std_srvs.srv import Empty, SetBool, Trigger 7 | 8 | from synchros2.scope import ROSAwareScope 9 | from synchros2.service_handle import ServiceHandle 10 | 11 | 12 | def do_request( 13 | client: Client, 14 | request: Type, 15 | label: Optional[str] = None, 16 | ) -> Tuple[bool, Optional[Any]]: 17 | if label is None: 18 | label = inspect.stack()[1].function 19 | assert client.wait_for_service(timeout_sec=1.0) 20 | 21 | failure = False 22 | 23 | def failure_callback() -> None: 24 | nonlocal failure 25 | failure = True 26 | 27 | handle = ServiceHandle(label) 28 | handle.set_on_failure_callback(failure_callback) 29 | 30 | service_future = client.call_async(request) 31 | handle.set_send_service_future(service_future) 32 | 33 | assert handle.wait(timeout_sec=1.0) 34 | return not failure, handle.result 35 | 36 | 37 | def test_successful_trigger(ros: ROSAwareScope) -> None: 38 | """Request a successful trigger. Should return a success condition with the message foo.""" 39 | 40 | def callback(req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response: 41 | result = Trigger.Response() 42 | result.success = True 43 | result.message = "foo" 44 | return result 45 | 46 | assert ros.node is not None 47 | ros.node.create_service(Trigger, "trigger_service", callback) 48 | client = ros.node.create_client(Trigger, "trigger_service") 49 | ok, result = do_request(client, Trigger.Request()) 50 | 51 | assert ok 52 | assert result is not None 53 | assert result.success 54 | assert result.message == "foo" 55 | 56 | 57 | def test_failed_trigger(ros: ROSAwareScope) -> None: 58 | """Request a failed trigger. Should return a success condition with the message bar.""" 59 | 60 | def callback(req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response: 61 | result = Trigger.Response() 62 | result.success = False 63 | result.message = "bar" 64 | return result 65 | 66 | assert ros.node is not None 67 | ros.node.create_service(Trigger, "trigger_service", callback) 68 | client = ros.node.create_client(Trigger, "trigger_service") 69 | 70 | ok, result = do_request(client, Trigger.Request()) 71 | 72 | assert not ok 73 | assert result is not None 74 | assert not result.success 75 | assert result.message == "bar" 76 | 77 | 78 | def setbool_service_callback(req: SetBool.Request, resp: SetBool.Response) -> SetBool.Response: 79 | result = SetBool.Response() 80 | if req.data: 81 | result.success = True 82 | result.message = "foo" 83 | else: 84 | result.success = False 85 | result.message = "bar" 86 | return result 87 | 88 | 89 | def test_successful_set_bool(ros: ROSAwareScope) -> None: 90 | """Request data passes a true value. Should return a success condition with the message foo.""" 91 | assert ros.node is not None 92 | ros.node.create_service(SetBool, "setbool_service", setbool_service_callback) 93 | client = ros.node.create_client(SetBool, "setbool_service") 94 | ok, result = do_request(client, SetBool.Request(data=True)) 95 | 96 | assert ok 97 | assert result is not None 98 | assert result.success 99 | assert result.message == "foo" 100 | 101 | 102 | def test_failed_set_bool(ros: ROSAwareScope) -> None: 103 | """Request data passes a false value. Should return a failure condition with the message bar.""" 104 | assert ros.node is not None 105 | ros.node.create_service(SetBool, "setbool_service", setbool_service_callback) 106 | client = ros.node.create_client(SetBool, "setbool_service") 107 | ok, result = do_request(client, SetBool.Request(data=False)) 108 | 109 | assert not ok 110 | assert result is not None 111 | assert not result.success 112 | assert result.message == "bar" 113 | 114 | 115 | def test_warning(ros: ROSAwareScope) -> None: 116 | """Tests that the result callback should give a warning and have no fields.""" 117 | 118 | def callback(req: Empty.Request, resp: Empty.Response) -> Empty.Response: 119 | return Empty.Response() 120 | 121 | assert ros.node is not None 122 | ros.node.create_service(Empty, "empty_service", callback) 123 | client = ros.node.create_client(Empty, "empty_service") 124 | ok, result = do_request(client, Empty.Request()) 125 | 126 | assert ok 127 | assert result is not None 128 | assert isinstance(result, Empty.Response) 129 | assert not hasattr(result, "success") 130 | -------------------------------------------------------------------------------- /synchros2/test/test_single_goal_action_server.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import array 3 | 4 | from example_interfaces.action import Fibonacci 5 | from rclpy.action.server import ServerGoalHandle 6 | 7 | from synchros2.action_client import ActionClientWrapper 8 | from synchros2.process import ROSAwareScope 9 | from synchros2.single_goal_action_server import SingleGoalActionServer 10 | 11 | 12 | def test_single_goal_action_server(ros: ROSAwareScope) -> None: 13 | """Tests normal operation of a single action server""" 14 | 15 | def execute_callback(goal_handle: ServerGoalHandle) -> Fibonacci.Result: 16 | """Executor for normal fibonacci sequence""" 17 | sequence = [0, 1] 18 | for i in range(1, goal_handle.request.order): 19 | sequence.append(sequence[i] + sequence[i - 1]) 20 | 21 | goal_handle.succeed() 22 | 23 | result = Fibonacci.Result() 24 | result.sequence = sequence 25 | return result 26 | 27 | assert ros.node is not None 28 | SingleGoalActionServer(ros.node, Fibonacci, "fibonacci", execute_callback) 29 | action_client = ActionClientWrapper(Fibonacci, "fibonacci", ros.node) 30 | 31 | goal = Fibonacci.Goal() 32 | goal.order = 5 33 | result = action_client.send_goal_and_wait("test_single_goal_action_server", goal=goal, timeout_sec=2) 34 | assert result is not None 35 | 36 | expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) 37 | assert result.sequence == expected_result 38 | -------------------------------------------------------------------------------- /synchros2/test/test_single_goal_multiple_action_servers.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import array 3 | from threading import Barrier, Lock 4 | from typing import Iterable, Tuple 5 | 6 | import pytest 7 | from example_interfaces.action import Fibonacci 8 | from rclpy.action.server import ServerGoalHandle 9 | from typing_extensions import TypeAlias 10 | 11 | from synchros2.action import Actionable 12 | from synchros2.futures import wait_for_future 13 | from synchros2.scope import ROSAwareScope 14 | from synchros2.single_goal_multiple_action_servers import SingleGoalMultipleActionServers 15 | 16 | 17 | @pytest.fixture 18 | def action_triplet(ros: ROSAwareScope) -> Iterable[Tuple[Barrier, Actionable, Actionable]]: 19 | lock = Lock() 20 | barrier = Barrier(2) 21 | 22 | def execute_callback(goal_handle: ServerGoalHandle) -> Fibonacci.Result: 23 | nonlocal barrier, lock 24 | 25 | if not barrier.broken: 26 | barrier.wait() 27 | 28 | with lock: 29 | sequence = [0, 1] 30 | for i in range(1, goal_handle.request.order): 31 | sequence.append(sequence[i] + sequence[i - 1]) 32 | 33 | if not barrier.broken: 34 | barrier.wait() 35 | 36 | result = Fibonacci.Result() 37 | if not goal_handle.is_cancel_requested: 38 | result.sequence = sequence 39 | goal_handle.succeed() 40 | else: 41 | goal_handle.canceled() 42 | return result 43 | 44 | def reversed_execute_callback(goal_handle: ServerGoalHandle) -> Fibonacci.Result: 45 | nonlocal barrier, lock 46 | 47 | if not barrier.broken: 48 | barrier.wait() 49 | 50 | with lock: 51 | sequence = [0, 1] 52 | for i in range(1, goal_handle.request.order): 53 | sequence.append(sequence[i] + sequence[i - 1]) 54 | 55 | if not barrier.broken: 56 | barrier.wait() 57 | 58 | result = Fibonacci.Result() 59 | if not goal_handle.is_cancel_requested: 60 | result.sequence = list(reversed(sequence)) 61 | goal_handle.succeed() 62 | else: 63 | goal_handle.canceled() 64 | return result 65 | 66 | action_parameters = [ 67 | (Fibonacci, "fibonacci/compute", execute_callback, None), 68 | (Fibonacci, "fibonacci/compute_reversed", reversed_execute_callback, None), 69 | ] 70 | assert ros.node is not None 71 | SingleGoalMultipleActionServers(ros.node, action_parameters, nosync=True) 72 | FibonacciActionable: TypeAlias = Actionable[Fibonacci.Goal, Fibonacci.Result, Fibonacci.Feedback] 73 | compute_fibonacci: FibonacciActionable = Actionable(Fibonacci, "fibonacci/compute", ros.node) 74 | compute_fibonacci_reversed: FibonacciActionable = Actionable(Fibonacci, "fibonacci/compute_reversed", ros.node) 75 | 76 | try: 77 | yield barrier, compute_fibonacci, compute_fibonacci_reversed 78 | finally: 79 | barrier.abort() 80 | 81 | 82 | def test_actions_in_sequence( 83 | action_triplet: Tuple[Barrier, Actionable, Actionable], 84 | ) -> None: 85 | barrier, compute_fibonacci, compute_fibonacci_reversed = action_triplet 86 | 87 | barrier.abort() # avoid synchronization 88 | 89 | goal = Fibonacci.Goal() 90 | goal.order = 5 91 | result = compute_fibonacci(goal) 92 | expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) 93 | assert result.sequence == expected_result 94 | result = compute_fibonacci_reversed(goal) 95 | expected_result = array.array("i", [5, 3, 2, 1, 1, 0]) 96 | assert result.sequence == expected_result 97 | 98 | 99 | def test_same_action_interruption( 100 | action_triplet: Tuple[Barrier, Actionable, Actionable], 101 | ) -> None: 102 | barrier, compute_fibonacci, _ = action_triplet 103 | 104 | goal = Fibonacci.Goal() 105 | goal.order = 5 106 | action_a = compute_fibonacci.asynchronously(goal) 107 | barrier.wait(timeout=5.0) # let action A start 108 | action_b = compute_fibonacci.asynchronously(goal) 109 | # Actions B and A will allow each other to start and finish, respectively 110 | assert wait_for_future(action_a.finalization, timeout_sec=5.0) 111 | assert action_a.cancelled 112 | barrier.wait(timeout=5.0) # let action B finish 113 | assert wait_for_future(action_b.finalization, timeout_sec=5.0) 114 | assert action_b.succeeded 115 | expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) 116 | assert action_b.result.sequence == expected_result 117 | 118 | 119 | def test_different_action_interruption( 120 | action_triplet: Tuple[Barrier, Actionable, Actionable], 121 | ) -> None: 122 | barrier, compute_fibonacci, compute_fibonacci_reversed = action_triplet 123 | 124 | goal = Fibonacci.Goal() 125 | goal.order = 5 126 | action_a = compute_fibonacci.asynchronously(goal) 127 | barrier.wait(timeout=5.0) # let action A start 128 | action_b = compute_fibonacci_reversed.asynchronously(goal) 129 | # Actions B and A will allow each other to start and finish, respectively 130 | assert wait_for_future(action_a.finalization, timeout_sec=5.0) 131 | assert action_a.cancelled 132 | barrier.wait(timeout=5.0) # let action B finish 133 | assert wait_for_future(action_b.finalization, timeout_sec=5.0) 134 | assert action_b.succeeded 135 | expected_result = array.array("i", [5, 3, 2, 1, 1, 0]) 136 | assert action_b.result.sequence == expected_result 137 | -------------------------------------------------------------------------------- /synchros2/test/test_static_transform_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | from geometry_msgs.msg import TransformStamped 4 | 5 | from synchros2.scope import ROSAwareScope 6 | from synchros2.static_transform_broadcaster import StaticTransformBroadcaster 7 | from synchros2.tf_listener_wrapper import TFListenerWrapper 8 | 9 | 10 | def test_static_tf_burst(ros: ROSAwareScope) -> None: 11 | """Asserts that all static transforms broadcasted in sequence are kept.""" 12 | assert ros.node is not None 13 | 14 | tf_broadcaster = StaticTransformBroadcaster(ros.node) 15 | 16 | stamp = ros.node.get_clock().now().to_msg() 17 | 18 | world_to_fiducial_a_transform = TransformStamped() 19 | world_to_fiducial_a_transform.header.stamp = stamp 20 | world_to_fiducial_a_transform.header.frame_id = "world" 21 | world_to_fiducial_a_transform.child_frame_id = "fiducial_a" 22 | world_to_fiducial_a_transform.transform.rotation.w = 1.0 23 | tf_broadcaster.sendTransform(world_to_fiducial_a_transform) 24 | 25 | body_to_head_transform = TransformStamped() 26 | body_to_head_transform.header.stamp = stamp 27 | body_to_head_transform.header.frame_id = "body" 28 | body_to_head_transform.child_frame_id = "head" 29 | body_to_head_transform.transform.rotation.z = -1.0 30 | 31 | head_to_camera_transform = TransformStamped() 32 | head_to_camera_transform.header.stamp = stamp 33 | head_to_camera_transform.header.frame_id = "head" 34 | head_to_camera_transform.child_frame_id = "camera" 35 | head_to_camera_transform.transform.rotation.z = 1.0 36 | tf_broadcaster.sendTransform([body_to_head_transform, head_to_camera_transform]) 37 | 38 | world_to_fiducial_a_transform = TransformStamped() 39 | world_to_fiducial_a_transform.header.stamp = stamp 40 | world_to_fiducial_a_transform.header.frame_id = "world" 41 | world_to_fiducial_a_transform.child_frame_id = "fiducial_a" 42 | world_to_fiducial_a_transform.transform.translation.x = 1.0 43 | world_to_fiducial_a_transform.transform.rotation.w = 1.0 44 | 45 | footprint_to_body_transform = TransformStamped() 46 | footprint_to_body_transform.header.stamp = stamp 47 | footprint_to_body_transform.header.frame_id = "footprint" 48 | footprint_to_body_transform.child_frame_id = "body" 49 | footprint_to_body_transform.transform.rotation.w = 1.0 50 | tf_broadcaster.sendTransform([world_to_fiducial_a_transform, footprint_to_body_transform]) 51 | 52 | tf_listener = TFListenerWrapper(ros.node) 53 | transform = tf_listener.lookup_a_tform_b("footprint", "camera", timeout_sec=2.0, wait_for_frames=True) 54 | assert transform.transform.rotation.w == 1.0 55 | transform = tf_listener.lookup_a_tform_b("world", "fiducial_a", timeout_sec=2.0, wait_for_frames=True) 56 | assert transform.transform.translation.x == 1.0 57 | -------------------------------------------------------------------------------- /synchros2/test/test_subscription.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import itertools 3 | import time 4 | from typing import Any, Iterator, cast 5 | 6 | from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile 7 | from std_msgs.msg import Int8, String 8 | 9 | from synchros2.futures import wait_for_future 10 | from synchros2.node import Node 11 | from synchros2.scope import ROSAwareScope 12 | from synchros2.subscription import Subscription, wait_for_message, wait_for_messages 13 | from synchros2.utilities import ensure 14 | 15 | DEFAULT_QOS_PROFILE = QoSProfile( 16 | durability=DurabilityPolicy.TRANSIENT_LOCAL, 17 | history=HistoryPolicy.KEEP_ALL, 18 | depth=1, 19 | ) 20 | 21 | 22 | def test_wait_for_message(ros: ROSAwareScope) -> None: 23 | """Asserts that waiting for a message works as expected.""" 24 | assert ros.node is not None 25 | pub = ros.node.create_publisher(Int8, "test", DEFAULT_QOS_PROFILE) 26 | assert not wait_for_message(Int8, "test", node=ros.node, timeout_sec=0.5) 27 | 28 | def deferred_publish() -> None: 29 | time.sleep(0.5) 30 | pub.publish(Int8(data=1)) 31 | 32 | assert ros.executor is not None 33 | ros.executor.create_task(deferred_publish) 34 | message = wait_for_message(Int8, "test", node=ros.node, timeout_sec=5.0) 35 | assert message is not None 36 | assert message.data == 1 37 | 38 | 39 | def test_subscription_matching_publishers(ros: ROSAwareScope) -> None: 40 | """Asserts that checking for publisher matching on a subscription works as expected.""" 41 | assert ros.node is not None 42 | sequence = Subscription(Int8, "sequence", DEFAULT_QOS_PROFILE, node=ros.node) 43 | assert sequence.matched_publishers == 0 44 | future = sequence.publisher_matches(1) 45 | assert not future.done() 46 | future.cancel() 47 | 48 | ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 49 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 50 | assert sequence.matched_publishers == 1 51 | 52 | 53 | def test_subscription_future_wait(ros: ROSAwareScope) -> None: 54 | """Asserts that waiting for a subscription update works as expected.""" 55 | assert ros.node is not None 56 | pub = ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 57 | sequence = Subscription(Int8, "sequence", DEFAULT_QOS_PROFILE, node=ros.node) 58 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 59 | assert sequence.matched_publishers == 1 60 | 61 | pub.publish(Int8(data=1)) 62 | 63 | assert wait_for_future(sequence.update, timeout_sec=5.0) 64 | assert cast(Int8, ensure(sequence.latest)).data == 1 65 | 66 | 67 | def test_subscription_matching_future_wait(ros: ROSAwareScope) -> None: 68 | """Asserts that waiting for a matching subscription update works as expected.""" 69 | assert ros.node is not None 70 | pub = ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 71 | sequence = Subscription(Int8, "sequence", DEFAULT_QOS_PROFILE, node=ros.node) 72 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 73 | assert sequence.matched_publishers == 1 74 | 75 | def deferred_publish() -> None: 76 | time.sleep(0.5) 77 | for num in range(5): 78 | pub.publish(Int8(data=num)) 79 | 80 | assert ros.executor is not None 81 | ros.executor.create_task(deferred_publish) 82 | 83 | future = sequence.matching_update(lambda message: message.data == 3) 84 | assert wait_for_future(future, timeout_sec=5.0) 85 | message = future.result() 86 | assert message.data == 3 87 | 88 | 89 | def test_subscription_iteration(ros: ROSAwareScope) -> None: 90 | """Asserts that iterating over subscription messages works as expected.""" 91 | assert ros.node is not None 92 | 93 | pub = ros.node.create_publisher( 94 | Int8, 95 | "sequence", 96 | DEFAULT_QOS_PROFILE, 97 | ) 98 | sequence = Subscription( 99 | Int8, 100 | "sequence", 101 | DEFAULT_QOS_PROFILE, 102 | history_length=3, 103 | node=ros.node, 104 | ) 105 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 106 | assert sequence.matched_publishers == 1 107 | 108 | expected_sequence_numbers = [1, 10, 100] 109 | 110 | def deferred_publish() -> None: 111 | time.sleep(0.5) 112 | for num in expected_sequence_numbers: 113 | pub.publish(Int8(data=num)) 114 | 115 | assert ros.executor is not None 116 | ros.executor.create_task(deferred_publish) 117 | message_stream: Iterator[Int8] = sequence.stream(timeout_sec=5.0) 118 | 119 | streamed_numbers = [msg.data for msg in itertools.islice(message_stream, 3)] 120 | assert expected_sequence_numbers == streamed_numbers 121 | 122 | historic_numbers = [msg.data for msg in sequence.history] 123 | assert expected_sequence_numbers == historic_numbers 124 | 125 | 126 | def test_deferred_start_subscription(ros: ROSAwareScope) -> None: 127 | """Asserts that deferred subscription start works as expected.""" 128 | assert ros.node is not None 129 | pub = ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 130 | sequence = Subscription( 131 | Int8, 132 | "sequence", 133 | DEFAULT_QOS_PROFILE, 134 | node=ros.node, 135 | autostart=False, 136 | ) 137 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 138 | assert sequence.matched_publishers == 1 139 | 140 | pub.publish(Int8(data=1)) 141 | 142 | future = sequence.update 143 | assert not future.done() 144 | sequence.start() 145 | 146 | assert wait_for_future(future, timeout_sec=5.0) 147 | assert cast(Int8, ensure(sequence.latest)).data == 1 148 | 149 | 150 | def test_subscription_cancelation(ros: ROSAwareScope) -> None: 151 | """Asserts that cancelling a subscription works as expected.""" 152 | assert ros.node is not None 153 | pub = ros.node.create_publisher(Int8, "sequence", DEFAULT_QOS_PROFILE) 154 | sequence = Subscription(Int8, "sequence", DEFAULT_QOS_PROFILE, node=ros.node) 155 | assert wait_for_future(sequence.publisher_matches(1), timeout_sec=5.0) 156 | assert sequence.matched_publishers == 1 157 | 158 | pub.publish(Int8(data=1)) 159 | 160 | assert wait_for_future(sequence.update, timeout_sec=5.0) 161 | 162 | def deferred_cancellation() -> None: 163 | time.sleep(0.5) 164 | sequence.unsubscribe() 165 | 166 | assert ros.executor is not None 167 | ros.executor.create_task(deferred_cancellation) 168 | 169 | streamed_numbers = [msg.data for msg in sequence.stream()] 170 | assert len(streamed_numbers) == 1 171 | assert streamed_numbers[0] == 1 172 | 173 | pub.publish(Int8(data=10)) 174 | 175 | assert not wait_for_future(sequence.update, timeout_sec=5.0) 176 | assert sequence.update.cancelled() 177 | 178 | historic_numbers = [msg.data for msg in sequence.history] 179 | assert len(historic_numbers) == 1 180 | assert historic_numbers[0] == 1 181 | 182 | assert cast(Int8, ensure(sequence.latest)).data == 1 183 | 184 | 185 | def test_wait_for_messages(ros: ROSAwareScope) -> None: 186 | """Asserts that waiting for multiple synchronized messages works as expected.""" 187 | 188 | class TestNode(Node): 189 | def __init__(self, name: str, **kwargs: Any) -> None: 190 | super().__init__(name, **kwargs) 191 | self.pub = self.create_publisher(String, "~/test", 10) 192 | self.timer = self.create_timer(0.5, self.do_publish) 193 | 194 | def do_publish(self) -> None: 195 | self.pub.publish(String(data=f"hello from {self.get_name()}")) 196 | 197 | ros.load(TestNode, "foo") 198 | ros.load(TestNode, "bar") 199 | 200 | messages = wait_for_messages( 201 | ["foo/test", "bar/test"], 202 | [String, String], 203 | timeout_sec=10.0, 204 | allow_headerless=True, 205 | delay=0.5, 206 | node=ros.node, 207 | ) 208 | assert messages is None or messages == ( 209 | String(data="hello from foo"), 210 | String(data="hello from bar"), 211 | ) 212 | 213 | messages = wait_for_messages( 214 | ["bar/test", "foo/test"], 215 | [String, String], 216 | allow_headerless=True, 217 | delay=0.5, 218 | timeout_sec=10.0, 219 | node=ros.node, 220 | ) 221 | assert messages is None or messages == ( 222 | String(data="hello from bar"), 223 | String(data="hello from foo"), 224 | ) 225 | 226 | # Test the case where the topic doesn't exist - timeout expected 227 | messages = wait_for_messages( 228 | ["/test", "foo/test"], 229 | [String, String], 230 | allow_headerless=True, 231 | timeout_sec=1.0, 232 | node=ros.node, 233 | ) 234 | assert messages is None 235 | -------------------------------------------------------------------------------- /synchros2/test/test_tf_listener_wrapper.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | import time 3 | from typing import Any, Iterable, Optional, Tuple 4 | 5 | import pytest 6 | from geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 7 | from rclpy.duration import Duration 8 | from rclpy.time import Time 9 | from tf2_ros import ExtrapolationException, LookupException, TransformBroadcaster 10 | 11 | from synchros2.node import Node 12 | from synchros2.scope import ROSAwareScope 13 | from synchros2.tf_listener_wrapper import TFListenerWrapper 14 | 15 | ROBOT = "test_robot" 16 | CAMERA = "camera_1" 17 | FRAME_ID = f"{ROBOT}/body" 18 | CHILD_FRAME_ID = f"{ROBOT}/{CAMERA}" 19 | 20 | 21 | def equal_transform(a: Transform, b: Transform) -> bool: 22 | return ( 23 | a.translation.x == b.translation.x 24 | and a.translation.y == b.translation.y 25 | and a.translation.z == b.translation.z 26 | and a.rotation.w == b.rotation.w 27 | and a.rotation.x == b.rotation.x 28 | and a.rotation.y == b.rotation.y 29 | and a.rotation.z == a.rotation.z 30 | ) 31 | 32 | 33 | class MockTfPublisherNode(Node): 34 | def __init__(self, frame_id: str, child_frame_id: str, **kwargs: Any) -> None: 35 | super().__init__("mock_tf_publisher", **kwargs) 36 | 37 | self._frame_id = frame_id 38 | self._child_frame_id = child_frame_id 39 | self._tf_broadcaster = TransformBroadcaster(self) 40 | 41 | def publish_transform(self, trans: Transform, timestamp: Optional[Time]) -> None: 42 | t = TransformStamped() 43 | 44 | if timestamp is not None: 45 | t.header.stamp = timestamp.to_msg() 46 | t.header.frame_id = self._frame_id 47 | t.child_frame_id = self._child_frame_id 48 | 49 | t.transform = trans 50 | 51 | self._tf_broadcaster.sendTransform(t) 52 | 53 | 54 | @pytest.fixture 55 | def tf_pair(ros: ROSAwareScope) -> Iterable[Tuple[MockTfPublisherNode, TFListenerWrapper]]: 56 | with ros.managed(MockTfPublisherNode, FRAME_ID, CHILD_FRAME_ID) as tf_publisher: 57 | assert ros.node is not None 58 | tf_listener = TFListenerWrapper(ros.node) 59 | yield tf_publisher, tf_listener 60 | 61 | 62 | def test_non_existant_transform(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: 63 | tf_publisher, tf_listener = tf_pair 64 | assert ros.node is not None 65 | timestamp = ros.node.get_clock().now() 66 | with pytest.raises(LookupException): 67 | tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp) 68 | 69 | 70 | def test_non_existant_transform_timeout( 71 | ros: ROSAwareScope, 72 | tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper], 73 | ) -> None: 74 | tf_publisher, tf_listener = tf_pair 75 | assert ros.node is not None 76 | timestamp = ros.node.get_clock().now() 77 | start = time.time() 78 | with pytest.raises(LookupException): 79 | tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=20.0) 80 | assert time.time() - start < 10.0 81 | 82 | 83 | def test_existing_transform(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: 84 | tf_publisher, tf_listener = tf_pair 85 | assert ros.node is not None 86 | timestamp = ros.node.get_clock().now() 87 | trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) 88 | tf_publisher.publish_transform(trans, timestamp) 89 | time.sleep(0.2) 90 | t = tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp) 91 | assert equal_transform(t.transform, trans) 92 | 93 | 94 | def test_future_transform_extrapolation_exception( 95 | ros: ROSAwareScope, 96 | tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper], 97 | ) -> None: 98 | tf_publisher, tf_listener = tf_pair 99 | assert ros.node is not None 100 | timestamp = ros.node.get_clock().now() 101 | trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) 102 | tf_publisher.publish_transform(trans, timestamp) 103 | time.sleep(0.2) 104 | timestamp = ros.node.get_clock().now() 105 | with pytest.raises(ExtrapolationException): 106 | tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.0) 107 | 108 | 109 | def test_future_transform_insufficient_wait( 110 | ros: ROSAwareScope, 111 | tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper], 112 | ) -> None: 113 | tf_publisher, tf_listener = tf_pair 114 | assert ros.node is not None 115 | timestamp = ros.node.get_clock().now() 116 | trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) 117 | tf_publisher.publish_transform(trans, timestamp) 118 | 119 | delay = 2 120 | 121 | def delayed_publish() -> None: 122 | time.sleep(delay) 123 | assert ros.node is not None 124 | delayed_timestamp = ros.node.get_clock().now() 125 | tf_publisher.publish_transform(trans, delayed_timestamp) 126 | 127 | assert ros.executor is not None 128 | ros.executor.create_task(delayed_publish) 129 | 130 | time.sleep(0.5) 131 | timestamp = ros.node.get_clock().now() + Duration(seconds=delay) 132 | with pytest.raises(ExtrapolationException): 133 | tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.5) 134 | 135 | 136 | def test_future_transform_wait(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: 137 | tf_publisher, tf_listener = tf_pair 138 | assert ros.node is not None 139 | timestamp = ros.node.get_clock().now() 140 | trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) 141 | tf_publisher.publish_transform(trans, timestamp) 142 | 143 | delay = 1 144 | 145 | def delayed_publish() -> None: 146 | time.sleep(delay + 0.001) 147 | delayed_timestamp = tf_publisher.get_clock().now() 148 | tf_publisher.publish_transform(trans, delayed_timestamp) 149 | 150 | assert ros.executor is not None 151 | ros.executor.create_task(delayed_publish) 152 | 153 | timestamp += Duration(seconds=delay) 154 | t = tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=2.0, wait_for_frames=True) 155 | assert equal_transform(t.transform, trans) 156 | 157 | 158 | def test_future_timestamp(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: 159 | tf_publisher, tf_listener = tf_pair 160 | timestamp = tf_publisher.get_clock().now() 161 | 162 | with pytest.raises(LookupException): 163 | tf_listener.lookup_latest_timestamp(FRAME_ID, CHILD_FRAME_ID) 164 | 165 | def delayed_publish() -> None: 166 | time.sleep(1.0) 167 | trans = Transform(rotation=Quaternion(w=1.0)) 168 | tf_publisher.publish_transform(trans, timestamp) 169 | 170 | assert ros.executor is not None 171 | ros.executor.create_task(delayed_publish) 172 | 173 | latest_timestamp = tf_listener.lookup_latest_timestamp( 174 | FRAME_ID, 175 | CHILD_FRAME_ID, 176 | timeout_sec=2.0, 177 | wait_for_frames=True, 178 | ) 179 | assert latest_timestamp == timestamp 180 | -------------------------------------------------------------------------------- /synchros2/test/test_utilities.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. 2 | 3 | import argparse 4 | import contextlib 5 | import itertools 6 | 7 | import pytest 8 | 9 | from synchros2.utilities import Tape, either_or, ensure, namespace_with 10 | 11 | 12 | def test_tape_head() -> None: 13 | tape: Tape[int] = Tape() 14 | assert tape.head is None 15 | expected_sequence = list(range(10)) 16 | for i in expected_sequence: 17 | tape.write(i) 18 | assert tape.head == i 19 | 20 | 21 | def test_tape_content_iteration() -> None: 22 | tape: Tape[int] = Tape() 23 | expected_sequence = list(range(10)) 24 | for i in expected_sequence: 25 | tape.write(i) 26 | assert list(tape.content()) == expected_sequence 27 | 28 | 29 | def test_tape_content_destructive_iteration() -> None: 30 | tape: Tape[int] = Tape() 31 | expected_sequence = list(range(10)) 32 | for i in expected_sequence: 33 | tape.write(i) 34 | assert list(tape.content(expunge=True)) == expected_sequence 35 | assert len(list(tape.content())) == 0 36 | 37 | 38 | def test_tape_content_greedy_iteration() -> None: 39 | tape: Tape[int] = Tape() 40 | expected_sequence = list(range(10)) 41 | for i in expected_sequence: 42 | tape.write(i) 43 | assert tape.content(greedy=True) == expected_sequence 44 | 45 | 46 | def test_tape_content_following() -> None: 47 | tape: Tape[int] = Tape() 48 | expected_sequence = list(range(10)) 49 | for i in expected_sequence: 50 | tape.write(i) 51 | with contextlib.closing(tape.content(follow=True)) as stream: 52 | assert list(itertools.islice(stream, 10)) == expected_sequence 53 | tape.write(10) 54 | assert next(stream) == 10 55 | 56 | 57 | def test_tape_content_greedy_following() -> None: 58 | tape: Tape[int] = Tape() 59 | expected_sequence = list(range(10)) 60 | for i in expected_sequence: 61 | tape.write(i) 62 | with contextlib.closing(tape.content(greedy=True, follow=True)) as stream: 63 | assert next(stream) == expected_sequence 64 | tape.write(10) 65 | tape.write(20) 66 | assert next(stream) == [10, 20] 67 | 68 | 69 | def test_tape_drops_unused_streams() -> None: 70 | tape: Tape[int] = Tape(max_length=0) 71 | 72 | stream = tape.content(follow=True) 73 | expected_value = 42 74 | tape.write(expected_value) 75 | value = next(stream) 76 | assert value == expected_value 77 | 78 | del stream 79 | 80 | assert len(tape._streams) == 0 81 | 82 | 83 | def test_tape_future_writes() -> None: 84 | tape: Tape[int] = Tape() 85 | tape.write(0) 86 | future = tape.future_write 87 | assert not future.done() 88 | tape.write(1) 89 | assert future.done() 90 | assert future.result() == 1 91 | tape.close() 92 | future = tape.future_write 93 | assert future.cancelled() 94 | 95 | 96 | def test_tape_latest_writes() -> None: 97 | tape: Tape[int] = Tape() 98 | assert tape.head is None 99 | future = tape.latest_write 100 | assert not future.done() 101 | tape.write(0) 102 | assert tape.head == 0 103 | assert future.done() 104 | assert future.result() == tape.head 105 | future = tape.latest_write 106 | assert future.done() 107 | assert future.result() == tape.head 108 | 109 | 110 | def test_either_or() -> None: 111 | assert either_or(None, "value", True) 112 | data = argparse.Namespace(value=True) 113 | assert either_or(data, "value", False) 114 | data = argparse.Namespace(value=True, getter=lambda obj: obj.value) 115 | assert either_or(data, "getter", False) 116 | 117 | 118 | def test_namespace_with() -> None: 119 | assert namespace_with(None, "foo") == "foo" 120 | assert namespace_with("", "foo") == "foo" 121 | assert namespace_with("/", "foo") == "/foo" 122 | assert namespace_with("foo", "bar") == "foo/bar" 123 | 124 | 125 | def test_ensure() -> None: 126 | data = None 127 | with pytest.raises(ValueError) as excinfo: 128 | ensure(data) 129 | assert "ensure(data) failed" in str(excinfo.value) 130 | --------------------------------------------------------------------------------