diff --git a/.github/workflows/check-samples.yml b/.github/workflows/check-samples.yml new file mode 100644 index 0000000..0a54ffb --- /dev/null +++ b/.github/workflows/check-samples.yml @@ -0,0 +1,132 @@ +name: Check samples + +# avoiding duplicate jobs on push with open pull_request: https://github.com/orgs/community/discussions/26940#discussioncomment-6656489 +on: [push, pull_request] + +jobs: + + check: + if: (github.event_name != 'pull_request' && ! github.event.pull_request.head.repo.fork) || (github.event_name == 'pull_request' && github.event.pull_request.head.repo.fork) + runs-on: ubuntu-latest + strategy: + matrix: + include: + - package_name: ros2_cpp_pkg + template: ros2_cpp_pkg + node_name: ros2_cpp_node + is_component: false + is_lifecycle: false + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: false + has_action_server: false + has_timer: false + has_docker_ros: true + - package_name: ros2_cpp_component_pkg + template: ros2_cpp_pkg + node_name: ros2_cpp_node + is_component: true + is_lifecycle: false + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: false + has_action_server: false + has_timer: false + has_docker_ros: true + - package_name: ros2_cpp_lifecycle_pkg + template: ros2_cpp_pkg + node_name: ros2_cpp_node + is_component: false + is_lifecycle: true + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: false + has_action_server: false + has_timer: false + has_docker_ros: true + - package_name: ros2_cpp_all_pkg + template: ros2_cpp_pkg + node_name: ros2_cpp_node + is_component: true + is_lifecycle: true + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: true + has_action_server: true + has_timer: true + has_docker_ros: true + - package_name: ros2_python_pkg + template: ros2_python_pkg + node_name: ros2_python_node + is_lifecycle: false + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: false + has_action_server: false + has_timer: false + has_docker_ros: true + - package_name: ros2_python_all_pkg + template: ros2_python_pkg + node_name: ros2_python_node + is_lifecycle: false + has_launch_file: true + has_params: true + has_subscriber: true + has_publisher: true + has_service_server: true + has_action_server: true + has_timer: true + has_docker_ros: true + - package_name: ros2_interfaces_pkg + template: ros2_interfaces_pkg + steps: + - name: Checkout code + uses: actions/checkout@v4 + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: "3.10" + - name: Install dependencies + run: | + pip install "copier~=9.2.0" "jinja2-strcase~=0.0.2" + - name: Configure git to run copier + run: | + git config --global user.name "dummy" + git config --global user.email "dummy@dummy.com" + - name: Generate packages + run: | + copier copy --trust --defaults \ + -d template=${{ matrix.template }} \ + -d package_name=${{ matrix.package_name }} \ + -d node_name=${{ matrix.node_name }} \ + -d is_component=${{ matrix.is_component }} \ + -d is_lifecycle=${{ matrix.is_lifecycle }} \ + -d has_launch_file=${{ matrix.has_launch_file }} \ + -d has_params=${{ matrix.has_params }} \ + -d has_subscriber=${{ matrix.has_subscriber }} \ + -d has_publisher=${{ matrix.has_publisher }} \ + -d has_service_server=${{ matrix.has_service_server }} \ + -d has_action_server=${{ matrix.has_action_server }} \ + -d has_timer=${{ matrix.has_timer }} \ + -d has_docker_ros=${{ matrix.has_docker_ros }} \ + . packages + - name: Check for repository changes + run: | + rm -rf samples/${{ matrix.package_name }}* + mv packages/${{ matrix.package_name }}* samples/ + if [[ ! -z "$(git status --porcelain)" ]]; then + echo "Sample generation resulted in changes to the repository" + git status + git diff + exit 1 + fi diff --git a/.github/workflows/generate-and-test.yml b/.github/workflows/generate-and-test.yml new file mode 100644 index 0000000..9af7bad --- /dev/null +++ b/.github/workflows/generate-and-test.yml @@ -0,0 +1,179 @@ +name: Generate and test packages + +# avoiding duplicate jobs on push with open pull_request: https://github.com/orgs/community/discussions/26940#discussioncomment-6656489 +on: [push, pull_request] + +jobs: + + generate: + if: (github.event_name != 'pull_request' && ! github.event.pull_request.head.repo.fork) || (github.event_name == 'pull_request' && github.event.pull_request.head.repo.fork) + runs-on: ubuntu-latest + strategy: + matrix: + include: + - package_name: default_cpp_pkg + template: ros2_cpp_pkg + auto_shutdown: true + - package_name: component_cpp_pkg + template: ros2_cpp_pkg + is_component: true + auto_shutdown: true + - package_name: lifecycle_cpp_pkg + template: ros2_cpp_pkg + is_lifecycle: true + auto_shutdown: true + - package_name: service_cpp_pkg + template: ros2_cpp_pkg + has_service_server: true + auto_shutdown: true + - package_name: action_cpp_pkg + template: ros2_cpp_pkg + has_action_server: true + auto_shutdown: true + - package_name: timer_cpp_pkg + template: ros2_cpp_pkg + has_timer: true + auto_shutdown: true + - package_name: no_params_cpp_pkg + template: ros2_cpp_pkg + has_params: false + auto_shutdown: true + - package_name: no_launch_file_cpp_pkg + template: ros2_cpp_pkg + has_launch_file: false + auto_shutdown: true + - package_name: all_cpp_pkg + template: ros2_cpp_pkg + is_component: true + is_lifecycle: true + has_service_server: true + has_action_server: true + has_timer: true + auto_shutdown: true + - package_name: default_python_pkg + template: ros2_python_pkg + auto_shutdown: true + - package_name: service_python_pkg + template: ros2_python_pkg + has_service_server: true + auto_shutdown: true + - package_name: action_python_pkg + template: ros2_python_pkg + has_action_server: true + auto_shutdown: true + - package_name: timer_python_pkg + template: ros2_python_pkg + has_timer: true + auto_shutdown: true + - package_name: no_params_python_pkg + template: ros2_python_pkg + has_params: false + auto_shutdown: true + - package_name: no_launch_file_python_pkg + template: ros2_python_pkg + has_launch_file: false + auto_shutdown: true + - package_name: all_python_pkg + template: ros2_python_pkg + is_lifecycle: true + has_service_server: true + has_action_server: true + has_timer: true + auto_shutdown: true + - package_name: interfaces_pkg + template: ros2_interfaces_pkg + steps: + - name: Checkout code + uses: actions/checkout@v4 + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: "3.10" + - name: Install dependencies + run: | + pip install "copier~=9.2.0" "jinja2-strcase~=0.0.2" + - name: Configure git to run copier + run: | + git config --global user.name "dummy" + git config --global user.email "dummy@dummy.com" + - name: Generate packages + run: | + copier copy --trust --defaults \ + -d template=${{ matrix.template }} \ + -d auto_shutdown=${{ matrix.auto_shutdown }} \ + -d package_name=${{ matrix.package_name }} \ + $( [ "${{ matrix.is_component }}" = "true" ] && echo "-d is_component=true" ) \ + $( [ "${{ matrix.is_lifecycle }}" = "true" ] && echo "-d is_lifecycle=true" ) \ + $( [ "${{ matrix.has_service_server }}" = "true" ] && echo "-d has_service_server=true" ) \ + $( [ "${{ matrix.has_action_server }}" = "true" ] && echo "-d has_action_server=true" ) \ + $( [ "${{ matrix.has_timer }}" = "true" ] && echo "-d has_timer=true" ) \ + $( [ "${{ matrix.has_params }}" = "false" ] && echo "-d has_params=false" ) \ + $( [ "${{ matrix.has_launch_file }}" = "false" ] && echo "-d has_launch_file=false" ) \ + . packages + - name: Upload artifacts + uses: actions/upload-artifact@v4 + with: + name: ${{ matrix.package_name }} + path: "packages/${{ matrix.package_name }}*" + + build: + if: (github.event_name != 'pull_request' && ! github.event.pull_request.head.repo.fork) || (github.event_name == 'pull_request' && github.event.pull_request.head.repo.fork) + runs-on: ubuntu-latest + container: + image: rwthika/ros2:jazzy + needs: generate + strategy: + matrix: + include: + - package: default_cpp_pkg + command: ros2 launch default_cpp_pkg default_cpp_pkg_launch.py + - package: component_cpp_pkg + command: ros2 launch component_cpp_pkg component_cpp_pkg_launch.py + - package: lifecycle_cpp_pkg + command: ros2 launch lifecycle_cpp_pkg lifecycle_cpp_pkg_launch.py + - package: service_cpp_pkg + command: ros2 launch service_cpp_pkg service_cpp_pkg_launch.py + - package: action_cpp_pkg + command: ros2 launch action_cpp_pkg action_cpp_pkg_launch.py + - package: timer_cpp_pkg + command: ros2 launch timer_cpp_pkg timer_cpp_pkg_launch.py + - package: no_params_cpp_pkg + command: ros2 launch no_params_cpp_pkg no_params_cpp_pkg_launch.py + - package: no_launch_file_cpp_pkg + command: ros2 run no_launch_file_cpp_pkg no_launch_file_cpp_pkg --ros-args -p param:=1.0 + - package: all_cpp_pkg + command: ros2 launch all_cpp_pkg all_cpp_pkg_launch.py + - package: default_python_pkg + command: ros2 launch default_python_pkg default_python_pkg_launch.py + - package: service_python_pkg + command: ros2 launch service_python_pkg service_python_pkg_launch.py + - package: action_python_pkg + command: ros2 launch action_python_pkg action_python_pkg_launch.py + - package: timer_python_pkg + command: ros2 launch timer_python_pkg timer_python_pkg_launch.py + - package: no_params_python_pkg + command: ros2 launch no_params_python_pkg no_params_python_pkg_launch.py + - package: no_launch_file_python_pkg + command: ros2 run no_launch_file_python_pkg no_launch_file_python_pkg --ros-args -p param:=1.0 + - package: all_python_pkg + command: ros2 launch all_python_pkg all_python_pkg_launch.py + - package: interfaces_pkg + command: | + ros2 interface show interfaces_pkg/msg/Message && \ + ros2 interface show interfaces_pkg/srv/Service && \ + ros2 interface show interfaces_pkg/action/Action + steps: + - name: Checkout code + uses: actions/checkout@v4 + - name: Download artifacts + uses: actions/download-artifact@v4 + - name: Build + shell: bash + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + colcon build --packages-up-to ${{ matrix.package }} + - name: Run + shell: bash + run: | + source install/setup.bash + ${{ matrix.command }} diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml new file mode 100644 index 0000000..843a804 --- /dev/null +++ b/.github/workflows/publish.yml @@ -0,0 +1,45 @@ +name: Publish to PyPI + +on: + push: + branches: + - main + tags: + - '*' + pull_request: + branches: + - main + +jobs: + publish: + name: Publish ${{ matrix.package }} + runs-on: ubuntu-latest + strategy: + matrix: + package: [ros2-pkg-create] + steps: + - name: Checkout repository + uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v5.2.0 + with: + python-version: "3.x" + - name: Install pypa/build + run: python3 -m pip install --user build + - name: Build wheel and tarball + run: python3 -m build --sdist --wheel --outdir dist/ ${{ matrix.package }} + - name: Publish to TestPyPI + uses: pypa/gh-action-pypi-publish@v1.10.1 + with: + password: ${{ secrets.TEST_PYPI_API_TOKEN }} + repository-url: https://test.pypi.org/legacy/ + skip-existing: true + - name: Publish to PyPI + if: startsWith(github.ref, 'refs/tags') + uses: pypa/gh-action-pypi-publish@v1.10.1 + with: + password: ${{ secrets.PYPI_API_TOKEN }} + skip-existing: true + # `continue-on-error` needed cause `skip-existing: true` is not honored + # https://github.com/pypa/gh-action-pypi-publish/issues/201 + continue-on-error: true \ No newline at end of file diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml deleted file mode 100644 index fe2ca3a..0000000 --- a/.gitlab-ci.yml +++ /dev/null @@ -1,113 +0,0 @@ -stages: - - generate - - ros2_cpp_pkg - - ros2_interfaces_pkg - - -generate: - stage: generate - image: python:3.10 - before_script: - - pip install "copier~=9.2.0" "jinja2-strcase~=0.0.2" - - git config --global user.name "dummy" - - git config --global user.email "dummy@dummy.com" - script: - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_default . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_component -d is_component=true . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_lifecycle -d is_lifecycle=true . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_service -d has_service_server=true . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_action -d has_action_server=true . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_timer -d has_timer=true . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_no_params -d has_params=false . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_no_launch_file -d has_launch_file=false . packages - - copier copy --trust --defaults -d template=ros2_cpp_pkg -d auto_shutdown=true -d package_name=pkg_all -d is_component=true -d is_lifecycle=true -d has_service_server=true -d has_action_server=true -d has_timer=true . packages - - copier copy --trust --defaults -d template=ros2_interfaces_pkg -d package_name=pkg_interfaces . packages - artifacts: - paths: - - packages/ - expire_in: 1 week - - -.build: - image: rwthika/ros2:latest - needs: [generate] - -.build-ros2_cpp_pkg: - extends: .build - stage: ros2_cpp_pkg - -.build-ros2_interfaces_pkg: - extends: .build - stage: ros2_interfaces_pkg - -pkg_default: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_default - - source install/setup.bash - - ros2 launch pkg_default pkg_default_launch.py - -pkg_component: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_component - - source install/setup.bash - - ros2 launch pkg_component pkg_component_launch.py - -pkg_lifecycle: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_lifecycle - - source install/setup.bash - - ros2 launch pkg_lifecycle pkg_lifecycle_launch.py - -pkg_service: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_service - - source install/setup.bash - - ros2 launch pkg_service pkg_service_launch.py - -pkg_action: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_action - - source install/setup.bash - - ros2 launch pkg_action pkg_action_launch.py - -pkg_timer: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_timer - - source install/setup.bash - - ros2 launch pkg_timer pkg_timer_launch.py - -pkg_no_params: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_no_params - - source install/setup.bash - - ros2 launch pkg_no_params pkg_no_params_launch.py - -pkg_no_launch_file: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_no_launch_file - - source install/setup.bash - - ros2 run pkg_no_launch_file pkg_no_launch_file --ros-args -p param:=1.0 - -pkg_all: - extends: .build-ros2_cpp_pkg - script: - - colcon build --packages-up-to pkg_all - - source install/setup.bash - - ros2 launch pkg_all pkg_all_launch.py - -pkg_interfaces: - extends: .build-ros2_interfaces_pkg - script: - - colcon build --packages-up-to pkg_interfaces - - source install/setup.bash - - ros2 interface show pkg_interfaces/msg/Message - - ros2 interface show pkg_interfaces/srv/Service - - ros2 interface show pkg_interfaces/action/Action diff --git a/README.md b/README.md index f214da6..c179534 100644 --- a/README.md +++ b/README.md @@ -1,20 +1,138 @@ -# ros2-pkg-create +# *ros2-pkg-create* – Powerful ROS 2 Package Generator -### Installation +

+ + + + +

+ +*ros2-pkg-create* is an interactive CLI tool for quickly generating ROS 2 packages from basic pub/sub nodes to complex lifecycle components. It is meant to replace the official [`ros2 pkg create`](https://docs.ros.org/en/latest/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html#create-a-package) command. + +- [Quick Demo](#quick-demo) +- [Installation](#installation) +- [Templates \& Features](#templates--features) +- [Usage](#usage) +- [Acknowledgements](#acknowledgements) + +> [!IMPORTANT] +> This repository is open-sourced and maintained by the [**Institute for Automotive Engineering (ika) at RWTH Aachen University**](https://www.ika.rwth-aachen.de/). +> **ROS is the backbone** of many research topics within our [*Vehicle Intelligence & Automated Driving*](https://www.ika.rwth-aachen.de/en/competences/fields-of-research/vehicle-intelligence-automated-driving.html) domain. +> If you would like to learn more about how we can support your advanced driver assistance and automated driving efforts, feel free to reach out to us! +> :email: ***opensource@ika.rwth-aachen.de*** + + +## Quick Demo + +```bash +pip install ros2-pkg-create +ros2-pkg-create --template ros2_cpp_pkg . +``` + + + + +## Installation ```bash -pip install --extra-index-url https://test.pypi.org/simple/ ros2-pkg-create +pip install ros2-pkg-create -# bash-completion +# (optional) bash auto-completion activate-global-python-argcomplete eval "$(register-python-argcomplete ros2-pkg-create)" ``` -### Usage +> [!WARNING] +> Outside of a virtual environment, *pip* may default to a user-site installation of executables to `~/.local/bin`, which may not be present in your shell's `PATH`. If running `ros2-pkg-create` errors with `ros2-pkg-create: command not found`, add the directory to your path. [*(More information)*](https://packaging.python.org/en/latest/tutorials/installing-packages/#installing-to-the-user-site) +> ```bash +> echo "export PATH=\$HOME/.local/bin:\$PATH" >> ~/.bashrc +> source ~/.bashrc +> ``` + + +## Templates & Features + +*ros2-pkg-create* provides multiple templates, each covering a different questionnaire for generating all the components you need. See below for the list of supported features and questionnarie options. Note that all options can also be passed directly to the command, bypassing the interactive questionnaire (see [Usage](#usage)). + +- [C++ Package](#c-package---template-ros2_cpp_pkg) +- [Python Package](#python-package---template-ros2_python_pkg) +- [Interfaces Package](#interfaces-package---template-ros2_interfaces_pkg) + +### C++ Package (`--template ros2_cpp_pkg`) + +**Supported Features:** publisher, subscriber, parameter loading, launch file, service server, action server, timer callback, component, lifecycle node, docker-ros + +
+Questionnaire + +- Package name +- Description +- Maintainer | Maintainer email +- Author | Author email +- License +- Node name +- Class name of node +- Make it a component? +- Make it a lifecycle node? +- Add a launch file? | Type of launch file +- Add parameter loading? +- Add a subscriber? +- Add a publisher? +- Add a service server? +- Add an action server? +- Add a timer callback? +- Add the docker-ros CI integration? +
+ +### Python Package (`--template ros2_python_pkg`) + +**Supported Features:** publisher, subscriber, parameter loading, launch file, service server, action server, timer callback, docker-ros + +
+Questionnaire + +- Package name +- Description +- Maintainer | Maintainer email +- Author | Author email +- License +- Node name +- Class name of node +- Add a launch file? | Type of launch file +- Add parameter loading? +- Add a subscriber? +- Add a publisher? +- Add a service server? +- Add an action server? +- Add a timer callback? +- Add the docker-ros CI integration? +
+ +### Interfaces Package (`--template ros2_interfaces_pkg`) + +**Supported Features:** message, service, action + +
+Questionnaire + +- Package name +- Description +- Maintainer | Maintainer email +- Author | Author email +- License +- Interfaces types +- Message name +- Service name +- Action name +- Add the docker-ros CI integration? +
+ + +## Usage ``` -usage: ros2-pkg-create [-h] [--defaults] --template {ros2_cpp_pkg,ros2_interfaces_pkg} [--package_name PACKAGE_NAME] [--description DESCRIPTION] [--maintainer MAINTAINER] - [--maintainer-email MAINTAINER_EMAIL] [--author AUTHOR] [--author-email AUTHOR_EMAIL] +usage: ros2-pkg-create [-h] [--defaults] [--use-local-templates] --template {ros2_interfaces_pkg,ros2_python_pkg,ros2_cpp_pkg} [--package-name PACKAGE_NAME] [--description DESCRIPTION] + [--maintainer MAINTAINER] [--maintainer-email MAINTAINER_EMAIL] [--author AUTHOR] [--author-email AUTHOR_EMAIL] [--license {Apache-2.0,BSL-1.0,BSD-2.0,BSD-2-Clause,BSD-3-Clause,GPL-3.0-only,LGPL-2.1-only,LGPL-3.0-only,MIT,MIT-0}] [--node-name NODE_NAME] [--node-class-name NODE_CLASS_NAME] [--is-component] [--no-is-component] [--is-lifecycle] [--no-is-lifecycle] [--has-launch-file] [--no-has-launch-file] [--launch-file-type {xml,py,yml}] [--has-params] [--no-has-params] [--has-subscriber] [--no-has-subscriber] [--has-publisher] [--no-has-publisher] @@ -23,7 +141,7 @@ usage: ros2-pkg-create [-h] [--defaults] --template {ros2_cpp_pkg,ros2_interface [--version] destination -Creates a ROS2 package from templates +Creates a ROS 2 package from templates positional arguments: destination Destination directory @@ -31,9 +149,11 @@ positional arguments: options: -h, --help show this help message and exit --defaults Use defaults for all options - --template {ros2_cpp_pkg,ros2_interfaces_pkg} + --use-local-templates + Use locally installed templates instead of remotely pulling most recent ones + --template {ros2_interfaces_pkg,ros2_python_pkg,ros2_cpp_pkg} Template - --package_name PACKAGE_NAME + --package-name PACKAGE_NAME Package name --description DESCRIPTION Description @@ -81,3 +201,7 @@ options: --has-docker-ros Add the docker-ros CI integration? --version show program's version number and exit ``` + +## Acknowledgements + +This work is accomplished within the projects [6GEM](https://6gem.de/en/) (FKZ 16KISK036K) and [autotech.agil](https://www.autotechagil.de/) (FKZ 01IS22088A). We acknowledge the financial support for the projects by the *Federal Ministry of Education and Research of Germany (BMBF)*. diff --git a/assets/cli.png b/assets/cli.png new file mode 100644 index 0000000..150e79d Binary files /dev/null and b/assets/cli.png differ diff --git a/copier.yml b/copier.yml index 887d50c..847ae16 100644 --- a/copier.yml +++ b/copier.yml @@ -12,12 +12,12 @@ template: help: Template type: str default: ros2_cpp_pkg - choices: [ros2_cpp_pkg, ros2_interfaces_pkg] + choices: [ros2_cpp_pkg, ros2_interfaces_pkg, ros2_python_pkg] package_name: help: Package name type: str - placeholder: ros2_cpp_pkg + placeholder: "{{ template }}" validator: "{% if not package_name %}Package name is required{% endif %}" description: @@ -53,13 +53,13 @@ license: node_name: help: Node name - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: str default: "{{ package_name }}" node_class_name: help: Class name of node - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: str default: "{{ node_name | to_camel }}" @@ -77,56 +77,56 @@ is_lifecycle: has_launch_file: help: Add a launch file? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: true launch_file_type: help: Type of launch file - when: "{{ template == 'ros2_cpp_pkg' and has_launch_file }}" + when: "{{ (template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg') and has_launch_file }}" type: str default: py choices: [py, xml, yml] has_params: help: Add parameter loading? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: true has_subscriber: help: Add a subscriber? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: true has_publisher: help: Add a publisher? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: true has_service_server: help: Add a service server? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: false has_action_server: help: Add an action server? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: false has_timer: help: Add a timer callback? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" type: bool default: false auto_shutdown: help: Automatically shutdown the node after launch (useful in CI/CD)? - when: "{{ template == 'ros2_cpp_pkg' }}" + when: "{{ template == 'ros2_cpp_pkg' or template == 'ros2_python_pkg' }}" when: false type: bool default: false diff --git a/ros2-pkg-create/pyproject.toml b/ros2-pkg-create/pyproject.toml index a6b1006..d90fbf7 100644 --- a/ros2-pkg-create/pyproject.toml +++ b/ros2-pkg-create/pyproject.toml @@ -4,15 +4,17 @@ build-backend = "setuptools.build_meta" [project] name = "ros2-pkg-create" -version = "0.0.8" -description = "TODO" +version = "1.0.0" +description = "Powerful ROS 2 Package Generator" license = {file = "LICENSE"} readme = "README.md" authors = [ {name = "Lennart Reiher", email = "lennart.reiher@rwth-aachen.de"}, + {name = "Jean-Pierre Busch", email = "jean-pierre.busch@rwth-aachen.de"}, ] maintainers = [ {name = "Lennart Reiher", email = "lennart.reiher@rwth-aachen.de"}, + {name = "Jean-Pierre Busch", email = "jean-pierre.busch@rwth-aachen.de"}, ] classifiers = [ "License :: OSI Approved :: MIT License", diff --git a/ros2-pkg-create/src/ros2_pkg_create/__init__.py b/ros2-pkg-create/src/ros2_pkg_create/__init__.py index 9a4db72..e7f535a 100644 --- a/ros2-pkg-create/src/ros2_pkg_create/__init__.py +++ b/ros2-pkg-create/src/ros2_pkg_create/__init__.py @@ -1,2 +1,2 @@ __name__ = "ros2-pkg-create" -__version__ = "0.0.8" \ No newline at end of file +__version__ = "1.0.0" \ No newline at end of file diff --git a/ros2-pkg-create/src/ros2_pkg_create/__main__.py b/ros2-pkg-create/src/ros2_pkg_create/__main__.py index 78d3ab2..3d367d2 100644 --- a/ros2-pkg-create/src/ros2_pkg_create/__main__.py +++ b/ros2-pkg-create/src/ros2_pkg_create/__main__.py @@ -9,13 +9,14 @@ def parseArguments() -> argparse.Namespace: - parser = argparse.ArgumentParser(description="Creates a ROS2 package from templates") + parser = argparse.ArgumentParser(description="Creates a ROS 2 package from templates") parser.add_argument("destination", type=str, help="Destination directory") parser.add_argument("--defaults", action="store_true", help="Use defaults for all options") + parser.add_argument("--use-local-templates", action="store_true", help="Use locally installed templates instead of remotely pulling most recent ones") - parser.add_argument("--template", type=str, default=None, choices=["ros2_cpp_pkg", "ros2_interfaces_pkg"], required=True, help="Template") - parser.add_argument("--package_name", type=str, default=None, help="Package name") + parser.add_argument("--template", type=str, default=None, choices=os.listdir(os.path.join(os.path.dirname(__file__), os.pardir, os.pardir, os.pardir, "templates")), required=True, help="Template") + parser.add_argument("--package-name", type=str, default=None, help="Package name") parser.add_argument("--description", type=str, default=None, help="Description") parser.add_argument("--maintainer", type=str, default=None, help="Maintainer") parser.add_argument("--maintainer-email", type=str, default=None, help="Maintainer email") @@ -65,8 +66,11 @@ def main(): # run copier try: - # local use: os.path.join(os.path.dirname(__file__), os.pardir, os.pardir, os.pardir), - copier.run_copy("https://gitlab.ika.rwth-aachen.de/fb-fi/ops/templates/ros2/ros2-pkg-create.git", + if args.use_local_templates: + template_location = os.path.join(os.path.dirname(__file__), os.pardir, os.pardir, os.pardir) + else: + template_location = "https://github.com/ika-rwth-aachen/ros2-pkg-create.git" + copier.run_copy(template_location, os.path.join(os.getcwd(), args.destination), data=answers, defaults=args.defaults, diff --git a/samples/generate_samples.sh b/samples/generate_samples.sh new file mode 100755 index 0000000..a000239 --- /dev/null +++ b/samples/generate_samples.sh @@ -0,0 +1,100 @@ +#!/bin/bash + +script_dir=$(dirname $0) + +copier copy --trust --defaults \ + -d template=ros2_cpp_pkg \ + -d package_name=ros2_cpp_pkg \ + -d node_name=ros2_cpp_node \ + -d is_component=false \ + -d is_lifecycle=false \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=false \ + -d has_action_server=false \ + -d has_timer=false \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_cpp_pkg \ + -d package_name=ros2_cpp_component_pkg \ + -d node_name=ros2_cpp_node \ + -d is_component=true \ + -d is_lifecycle=false \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=false \ + -d has_action_server=false \ + -d has_timer=false \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_cpp_pkg \ + -d package_name=ros2_cpp_lifecycle_pkg \ + -d node_name=ros2_cpp_node \ + -d is_component=false \ + -d is_lifecycle=true \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=false \ + -d has_action_server=false \ + -d has_timer=false \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_cpp_pkg \ + -d package_name=ros2_cpp_all_pkg \ + -d node_name=ros2_cpp_node \ + -d is_component=true \ + -d is_lifecycle=true \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=true \ + -d has_action_server=true \ + -d has_timer=true \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_python_pkg \ + -d package_name=ros2_python_pkg \ + -d node_name=ros2_python_node \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=false \ + -d has_action_server=false \ + -d has_timer=false \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_python_pkg \ + -d package_name=ros2_python_all_pkg \ + -d node_name=ros2_python_node \ + -d has_launch_file=true \ + -d has_params=true \ + -d has_subscriber=true \ + -d has_publisher=true \ + -d has_service_server=true \ + -d has_action_server=true \ + -d has_timer=true \ + -d has_docker_ros=true \ + . $script_dir + +copier copy --trust --defaults \ + -d template=ros2_interfaces_pkg \ + -d package_name=ros2_interfaces_pkg \ + . $script_dir diff --git a/samples/ros2_cpp_all_pkg/.gitlab-ci.yml b/samples/ros2_cpp_all_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..f642e55 --- /dev/null +++ b/samples/ros2_cpp_all_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_cpp_all_pkg ros2_cpp_node diff --git a/samples/ros2_cpp_all_pkg/CMakeLists.txt b/samples/ros2_cpp_all_pkg/CMakeLists.txt new file mode 100644 index 0000000..d8a7367 --- /dev/null +++ b/samples/ros2_cpp_all_pkg/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_cpp_all_pkg) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(ros2_cpp_all_pkg_interfaces REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +set(TARGET_NAME ros2_cpp_node_component) + +add_library(${TARGET_NAME} SHARED src/ros2_cpp_node.cpp) + +rclcpp_components_register_node(${TARGET_NAME} + PLUGIN "ros2_cpp_all_pkg::Ros2CppNode" + EXECUTABLE ros2_cpp_node +) + +target_include_directories(${TARGET_NAME} PUBLIC + $ + $ +) +target_compile_features(${TARGET_NAME} PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(${TARGET_NAME} + rclcpp + rclcpp_action + ros2_cpp_all_pkg_interfaces + rclcpp_components + rclcpp_lifecycle + std_msgs + std_srvs +) + +install(TARGETS ${TARGET_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +ament_package() diff --git a/samples/ros2_cpp_all_pkg/README.md b/samples/ros2_cpp_all_pkg/README.md new file mode 100644 index 0000000..8971d96 --- /dev/null +++ b/samples/ros2_cpp_all_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_cpp_all_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_cpp_node](#ros2_cpp_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_cpp_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_cpp_all_pkg/config/params.yml b/samples/ros2_cpp_all_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_cpp_all_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_cpp_all_pkg/include/ros2_cpp_all_pkg/ros2_cpp_node.hpp b/samples/ros2_cpp_all_pkg/include/ros2_cpp_all_pkg/ros2_cpp_node.hpp new file mode 100644 index 0000000..cf591f3 --- /dev/null +++ b/samples/ros2_cpp_all_pkg/include/ros2_cpp_all_pkg/ros2_cpp_node.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +namespace ros2_cpp_all_pkg { + +template struct is_vector : std::false_type {}; +template struct is_vector< std::vector > : std::true_type {}; +template inline constexpr bool is_vector_v = is_vector::value; + + +class Ros2CppNode : public rclcpp_lifecycle::LifecycleNode { + + public: + + explicit Ros2CppNode(const rclcpp::NodeOptions& options); + + protected: + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override; + + private: + + template + void declareAndLoadParameter(const std::string &name, + T ¶m, + const std::string &description, + const bool add_to_auto_reconfigurable_params = true, + const bool is_required = false, + const bool read_only = false, + const std::optional &from_value = std::nullopt, + const std::optional &to_value = std::nullopt, + const std::optional &step_value = std::nullopt, + const std::string &additional_constraints = ""); + + rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector& parameters); + + void setup(); + + void cleanUp(); + + void topicCallback(const std_msgs::msg::Int32& msg); + + void serviceCallback(const std::shared_ptrrequest, std::shared_ptr response); + + rclcpp_action::GoalResponse actionHandleGoal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse actionHandleCancel(const std::shared_ptr> goal_handle); + + void actionHandleAccepted(const std::shared_ptr> goal_handle); + + void actionExecute(const std::shared_ptr> goal_handle); + + void timerCallback(); + + private: + + std::vector>> auto_reconfigurable_params_; + + OnSetParametersCallbackHandle::SharedPtr parameters_callback_; + + rclcpp::Subscription::SharedPtr subscriber_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + + rclcpp::Service::SharedPtr service_server_; + + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp::TimerBase::SharedPtr timer_; + + double param_ = 1.0; +}; + + +} diff --git a/samples/ros2_cpp_all_pkg/launch/ros2_cpp_node_launch.py b/samples/ros2_cpp_all_pkg/launch/ros2_cpp_node_launch.py new file mode 100644 index 0000000..9e9b252 --- /dev/null +++ b/samples/ros2_cpp_all_pkg/launch/ros2_cpp_node_launch.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LifecycleNode, SetParameter + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_cpp_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_cpp_all_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + DeclareLaunchArgument("startup_state", default_value="None", description="initial lifecycle state"), + ] + + nodes = [ + GroupAction( + actions=[ + SetParameter( + name="startup_state", + value=LaunchConfiguration("startup_state"), + condition=LaunchConfigurationNotEquals("startup_state", "None") + ), + LifecycleNode( + package="ros2_cpp_all_pkg", + executable="ros2_cpp_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_cpp_all_pkg/package.xml b/samples/ros2_cpp_all_pkg/package.xml new file mode 100644 index 0000000..e5f4d0c --- /dev/null +++ b/samples/ros2_cpp_all_pkg/package.xml @@ -0,0 +1,28 @@ + + + + + ros2_cpp_all_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + rclcpp_lifecycle + std_msgs + std_srvs + ros2_cpp_all_pkg_interfaces + + + ament_cmake + + + diff --git a/samples/ros2_cpp_all_pkg/src/ros2_cpp_node.cpp b/samples/ros2_cpp_all_pkg/src/ros2_cpp_node.cpp new file mode 100644 index 0000000..c6fb53d --- /dev/null +++ b/samples/ros2_cpp_all_pkg/src/ros2_cpp_node.cpp @@ -0,0 +1,400 @@ +#include +#include +#include +#include + +#include + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_cpp_all_pkg::Ros2CppNode) + + +namespace ros2_cpp_all_pkg { + + +/** + * @brief Constructor + * + * @param options node options + */ +Ros2CppNode::Ros2CppNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("ros2_cpp_node", options) { + + int startup_state = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + this->declareAndLoadParameter("startup_state", startup_state, "Initial lifecycle state", false, false, false); + if (startup_state > lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (startup_state > lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); +} + + +/** + * @brief Declares and loads a ROS parameter + * + * @param name name + * @param param parameter variable to load into + * @param description description + * @param add_to_auto_reconfigurable_params enable reconfiguration of parameter + * @param is_required whether failure to load parameter will stop node + * @param read_only set parameter to read-only + * @param from_value parameter range minimum + * @param to_value parameter range maximum + * @param step_value parameter range step + * @param additional_constraints additional constraints description + */ +template +void Ros2CppNode::declareAndLoadParameter(const std::string& name, + T& param, + const std::string& description, + const bool add_to_auto_reconfigurable_params, + const bool is_required, + const bool read_only, + const std::optional& from_value, + const std::optional& to_value, + const std::optional& step_value, + const std::string& additional_constraints) { + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = description; + param_desc.additional_constraints = additional_constraints; + param_desc.read_only = read_only; + + auto type = rclcpp::ParameterValue(param).get_type(); + + if (from_value.has_value() && to_value.has_value()) { + if constexpr(std::is_integral_v) { + rcl_interfaces::msg::IntegerRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.integer_range = {range}; + } else if constexpr(std::is_floating_point_v) { + rcl_interfaces::msg::FloatingPointRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1.0); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.floating_point_range = {range}; + } else { + RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str()); + } + } + + this->declare_parameter(name, type, param_desc); + + try { + param = this->get_parameter(name).get_value(); + std::stringstream ss; + ss << "Loaded parameter '" << name << "': "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_INFO_STREAM(this->get_logger(), ss.str()); + } catch (rclcpp::exceptions::ParameterUninitializedException&) { + if (is_required) { + RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting"); + exit(EXIT_FAILURE); + } else { + std::stringstream ss; + ss << "Missing parameter '" << name << "', using default value: "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_WARN_STREAM(this->get_logger(), ss.str()); + } + } + + if (add_to_auto_reconfigurable_params) { + std::function setter = [¶m](const rclcpp::Parameter& p) { + param = p.get_value(); + }; + auto_reconfigurable_params_.push_back(std::make_tuple(name, setter)); + } +} + + +/** + * @brief Handles reconfiguration when a parameter value is changed + * + * @param parameters parameters + * @return parameter change result + */ +rcl_interfaces::msg::SetParametersResult Ros2CppNode::parametersCallback(const std::vector& parameters) { + + for (const auto& param : parameters) { + for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) { + if (param.get_name() == std::get<0>(auto_reconfigurable_param)) { + std::get<1>(auto_reconfigurable_param)(param); + RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str()); + break; + } + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + return result; +} + + +/** + * @brief Sets up subscribers, publishers, etc. to configure the node + */ +void Ros2CppNode::setup() { + + // callback for dynamic parameter configuration + parameters_callback_ = this->add_on_set_parameters_callback(std::bind(&Ros2CppNode::parametersCallback, this, std::placeholders::_1)); + + // subscriber for handling incoming messages + subscriber_ = this->create_subscription("~/input", 10, std::bind(&Ros2CppNode::topicCallback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", subscriber_->get_topic_name()); + + // publisher for publishing outgoing messages + publisher_ = this->create_publisher("~/output", 10); + RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", publisher_->get_topic_name()); + + // service server for handling service calls + service_server_ = this->create_service("~/service", std::bind(&Ros2CppNode::serviceCallback, this, std::placeholders::_1, std::placeholders::_2)); + + // action server for handling action goal requests + action_server_ = rclcpp_action::create_server( + this, + "~/action", + std::bind(&Ros2CppNode::actionHandleGoal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&Ros2CppNode::actionHandleCancel, this, std::placeholders::_1), + std::bind(&Ros2CppNode::actionHandleAccepted, this, std::placeholders::_1) + ); +} + + +/** + * @brief Processes messages received by a subscriber + * + * @param msg message + */ +void Ros2CppNode::topicCallback(const std_msgs::msg::Int32& msg) { + + RCLCPP_INFO(this->get_logger(), "Message received: '%d'", msg.data); +} + + +/** + * @brief Processes service requests + * + * @param request service request + * @param response service response + */ +void Ros2CppNode::serviceCallback(const std_srvs::srv::SetBool::Request::SharedPtr request, std_srvs::srv::SetBool::Response::SharedPtr response) { + + (void)request; + + RCLCPP_INFO(this->get_logger(), "Received service request"); + + response->success = true; +} + + +/** + * @brief Processes action goal requests + * + * @param uuid unique goal identifier + * @param goal action goal + * @return goal response + */ +rclcpp_action::GoalResponse Ros2CppNode::actionHandleGoal(const rclcpp_action::GoalUUID& uuid, ros2_cpp_all_pkg_interfaces::action::Fibonacci::Goal::ConstSharedPtr goal) { + + (void)uuid; + (void)goal; + + RCLCPP_INFO(this->get_logger(), "Received action goal request"); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + + +/** + * @brief Processes action cancel requests + * + * @param goal_handle action goal handle + * @return cancel response + */ +rclcpp_action::CancelResponse Ros2CppNode::actionHandleCancel(const std::shared_ptr> goal_handle) { + + (void)goal_handle; + + RCLCPP_INFO(this->get_logger(), "Received request to cancel action goal"); + + return rclcpp_action::CancelResponse::ACCEPT; +} + + +/** + * @brief Processes accepted action goal requests + * + * @param goal_handle action goal handle + */ +void Ros2CppNode::actionHandleAccepted(const std::shared_ptr> goal_handle) { + + // execute action in a separate thread to avoid blocking + std::thread{std::bind(&Ros2CppNode::actionExecute, this, std::placeholders::_1), goal_handle}.detach(); +} + + +/** + * @brief Executes an action + * + * @param goal_handle action goal handle + */ +void Ros2CppNode::actionExecute(const std::shared_ptr> goal_handle) { + + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + + // define a sleeping rate between computing individual Fibonacci numbers + rclcpp::Rate loop_rate(1); + + // create handy accessors for the action goal, feedback, and result + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + // initialize the Fibonacci sequence + auto& partial_sequence = feedback->partial_sequence; + partial_sequence.push_back(0); + partial_sequence.push_back(1); + + // compute the Fibonacci sequence up to the requested order n + for (int i = 1; i < goal->order && rclcpp::ok(); ++i) { + + // cancel, if requested + if (goal_handle->is_canceling()) { + result->sequence = feedback->partial_sequence; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Action goal canceled"); + return; + } + + // compute the next Fibonacci number + partial_sequence.push_back(partial_sequence[i] + partial_sequence[i - 1]); + + // publish the current sequence as action feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publishing action feedback"); + + // sleep before computing the next Fibonacci number + loop_rate.sleep(); + } + + // finish by publishing the action result + if (rclcpp::ok()) { + result->sequence = partial_sequence; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } +} + + +/** + * @brief Processes timer triggers + */ +void Ros2CppNode::timerCallback() { + + RCLCPP_INFO(this->get_logger(), "Timer triggered"); +} + + +/** + * @brief Processes 'configuring' transitions to 'inactive' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_configure(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Configuring to enter 'inactive' state from '%s' state", state.label().c_str()); + + this->declareAndLoadParameter("param", param_, "TODO", true, false, false, 0.0, 10.0, 1.0); + setup(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'activating' transitions to 'active' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_activate(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Activating to enter 'active' state from '%s' state", state.label().c_str()); + + publisher_->on_activate(); + timer_ = this->create_wall_timer(std::chrono::duration(param_), std::bind(&Ros2CppNode::timerCallback, this)); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'deactivating' transitions to 'inactive' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_deactivate(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Deactivating to enter 'inactive' state from '%s' state", state.label().c_str()); + + timer_.reset(); + publisher_->on_deactivate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'cleningup' transitions to 'unconfigured' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_cleanup(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Cleaning up to enter 'unconfigured' state from '%s' state", state.label().c_str()); + + subscriber_.reset(); + publisher_.reset(); + service_server_.reset(); + action_server_.reset(); + parameters_callback_.reset(); + timer_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'shuttingdown' transitions to 'finalized' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_shutdown(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Shutting down to enter 'finalized' state from '%s' state", state.label().c_str()); + + if (state.id() >= lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + on_deactivate(state); + if (state.id() >= lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + on_cleanup(state); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +} diff --git a/samples/ros2_cpp_all_pkg_interfaces/CMakeLists.txt b/samples/ros2_cpp_all_pkg_interfaces/CMakeLists.txt new file mode 100644 index 0000000..8d2a48d --- /dev/null +++ b/samples/ros2_cpp_all_pkg_interfaces/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_cpp_all_pkg_interfaces) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + action/Fibonacci.action +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/samples/ros2_cpp_all_pkg_interfaces/README.md b/samples/ros2_cpp_all_pkg_interfaces/README.md new file mode 100644 index 0000000..599dcf8 --- /dev/null +++ b/samples/ros2_cpp_all_pkg_interfaces/README.md @@ -0,0 +1,22 @@ +# ros2_cpp_all_pkg_interfaces + +ROS interface definitions for ros2_cpp_all_pkg + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/samples/ros2_cpp_all_pkg_interfaces/action/Fibonacci.action b/samples/ros2_cpp_all_pkg_interfaces/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/samples/ros2_cpp_all_pkg_interfaces/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/samples/ros2_cpp_all_pkg_interfaces/package.xml b/samples/ros2_cpp_all_pkg_interfaces/package.xml new file mode 100644 index 0000000..cb83e42 --- /dev/null +++ b/samples/ros2_cpp_all_pkg_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + + ros2_cpp_all_pkg_interfaces + 0.0.0 + ROS interface definitions for ros2_cpp_all_pkg + + TODO + TODO + + TODO + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/samples/ros2_cpp_component_pkg/.gitlab-ci.yml b/samples/ros2_cpp_component_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..e1fbb8d --- /dev/null +++ b/samples/ros2_cpp_component_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_cpp_component_pkg ros2_cpp_node diff --git a/samples/ros2_cpp_component_pkg/CMakeLists.txt b/samples/ros2_cpp_component_pkg/CMakeLists.txt new file mode 100644 index 0000000..a73587b --- /dev/null +++ b/samples/ros2_cpp_component_pkg/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_cpp_component_pkg) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) + +set(TARGET_NAME ros2_cpp_node_component) + +add_library(${TARGET_NAME} SHARED src/ros2_cpp_node.cpp) + +rclcpp_components_register_node(${TARGET_NAME} + PLUGIN "ros2_cpp_component_pkg::Ros2CppNode" + EXECUTABLE ros2_cpp_node +) + +target_include_directories(${TARGET_NAME} PUBLIC + $ + $ +) +target_compile_features(${TARGET_NAME} PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(${TARGET_NAME} + rclcpp + rclcpp_components + std_msgs +) + +install(TARGETS ${TARGET_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +ament_package() diff --git a/samples/ros2_cpp_component_pkg/README.md b/samples/ros2_cpp_component_pkg/README.md new file mode 100644 index 0000000..b42cd02 --- /dev/null +++ b/samples/ros2_cpp_component_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_cpp_component_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_cpp_node](#ros2_cpp_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_cpp_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_cpp_component_pkg/config/params.yml b/samples/ros2_cpp_component_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_cpp_component_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_cpp_component_pkg/include/ros2_cpp_component_pkg/ros2_cpp_node.hpp b/samples/ros2_cpp_component_pkg/include/ros2_cpp_component_pkg/ros2_cpp_node.hpp new file mode 100644 index 0000000..9ebb459 --- /dev/null +++ b/samples/ros2_cpp_component_pkg/include/ros2_cpp_component_pkg/ros2_cpp_node.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include + +#include +#include + + +namespace ros2_cpp_component_pkg { + +template struct is_vector : std::false_type {}; +template struct is_vector< std::vector > : std::true_type {}; +template inline constexpr bool is_vector_v = is_vector::value; + + +class Ros2CppNode : public rclcpp::Node { + + public: + + explicit Ros2CppNode(const rclcpp::NodeOptions& options); + + private: + + template + void declareAndLoadParameter(const std::string &name, + T ¶m, + const std::string &description, + const bool add_to_auto_reconfigurable_params = true, + const bool is_required = false, + const bool read_only = false, + const std::optional &from_value = std::nullopt, + const std::optional &to_value = std::nullopt, + const std::optional &step_value = std::nullopt, + const std::string &additional_constraints = ""); + + rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector& parameters); + + void setup(); + + void topicCallback(const std_msgs::msg::Int32& msg); + + private: + + std::vector>> auto_reconfigurable_params_; + + OnSetParametersCallbackHandle::SharedPtr parameters_callback_; + + rclcpp::Subscription::SharedPtr subscriber_; + + rclcpp::Publisher::SharedPtr publisher_; + + double param_ = 1.0; +}; + + +} diff --git a/samples/ros2_cpp_component_pkg/launch/ros2_cpp_node_launch.py b/samples/ros2_cpp_component_pkg/launch/ros2_cpp_node_launch.py new file mode 100644 index 0000000..c1cb24f --- /dev/null +++ b/samples/ros2_cpp_component_pkg/launch/ros2_cpp_node_launch.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_cpp_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_cpp_component_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + ] + + nodes = [ + Node( + package="ros2_cpp_component_pkg", + executable="ros2_cpp_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_cpp_component_pkg/package.xml b/samples/ros2_cpp_component_pkg/package.xml new file mode 100644 index 0000000..09e4c63 --- /dev/null +++ b/samples/ros2_cpp_component_pkg/package.xml @@ -0,0 +1,24 @@ + + + + + ros2_cpp_component_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + ament_cmake + + rclcpp + rclcpp_components + std_msgs + + + ament_cmake + + + diff --git a/samples/ros2_cpp_component_pkg/src/ros2_cpp_node.cpp b/samples/ros2_cpp_component_pkg/src/ros2_cpp_node.cpp new file mode 100644 index 0000000..9a59593 --- /dev/null +++ b/samples/ros2_cpp_component_pkg/src/ros2_cpp_node.cpp @@ -0,0 +1,166 @@ +#include + +#include + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_cpp_component_pkg::Ros2CppNode) + + +namespace ros2_cpp_component_pkg { + + +/** + * @brief Constructor + * + * @param options node options + */ +Ros2CppNode::Ros2CppNode(const rclcpp::NodeOptions& options) : Node("ros2_cpp_node", options) { + + this->declareAndLoadParameter("param", param_, "TODO", true, false, false, 0.0, 10.0, 1.0); + this->setup(); +} + + +/** + * @brief Declares and loads a ROS parameter + * + * @param name name + * @param param parameter variable to load into + * @param description description + * @param add_to_auto_reconfigurable_params enable reconfiguration of parameter + * @param is_required whether failure to load parameter will stop node + * @param read_only set parameter to read-only + * @param from_value parameter range minimum + * @param to_value parameter range maximum + * @param step_value parameter range step + * @param additional_constraints additional constraints description + */ +template +void Ros2CppNode::declareAndLoadParameter(const std::string& name, + T& param, + const std::string& description, + const bool add_to_auto_reconfigurable_params, + const bool is_required, + const bool read_only, + const std::optional& from_value, + const std::optional& to_value, + const std::optional& step_value, + const std::string& additional_constraints) { + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = description; + param_desc.additional_constraints = additional_constraints; + param_desc.read_only = read_only; + + auto type = rclcpp::ParameterValue(param).get_type(); + + if (from_value.has_value() && to_value.has_value()) { + if constexpr(std::is_integral_v) { + rcl_interfaces::msg::IntegerRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.integer_range = {range}; + } else if constexpr(std::is_floating_point_v) { + rcl_interfaces::msg::FloatingPointRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1.0); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.floating_point_range = {range}; + } else { + RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str()); + } + } + + this->declare_parameter(name, type, param_desc); + + try { + param = this->get_parameter(name).get_value(); + std::stringstream ss; + ss << "Loaded parameter '" << name << "': "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_INFO_STREAM(this->get_logger(), ss.str()); + } catch (rclcpp::exceptions::ParameterUninitializedException&) { + if (is_required) { + RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting"); + exit(EXIT_FAILURE); + } else { + std::stringstream ss; + ss << "Missing parameter '" << name << "', using default value: "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_WARN_STREAM(this->get_logger(), ss.str()); + } + } + + if (add_to_auto_reconfigurable_params) { + std::function setter = [¶m](const rclcpp::Parameter& p) { + param = p.get_value(); + }; + auto_reconfigurable_params_.push_back(std::make_tuple(name, setter)); + } +} + + +/** + * @brief Handles reconfiguration when a parameter value is changed + * + * @param parameters parameters + * @return parameter change result + */ +rcl_interfaces::msg::SetParametersResult Ros2CppNode::parametersCallback(const std::vector& parameters) { + + for (const auto& param : parameters) { + for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) { + if (param.get_name() == std::get<0>(auto_reconfigurable_param)) { + std::get<1>(auto_reconfigurable_param)(param); + RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str()); + break; + } + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + return result; +} + + +/** + * @brief Sets up subscribers, publishers, etc. to configure the node + */ +void Ros2CppNode::setup() { + + // callback for dynamic parameter configuration + parameters_callback_ = this->add_on_set_parameters_callback(std::bind(&Ros2CppNode::parametersCallback, this, std::placeholders::_1)); + + // subscriber for handling incoming messages + subscriber_ = this->create_subscription("~/input", 10, std::bind(&Ros2CppNode::topicCallback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", subscriber_->get_topic_name()); + + // publisher for publishing outgoing messages + publisher_ = this->create_publisher("~/output", 10); + RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", publisher_->get_topic_name()); +} + + +/** + * @brief Processes messages received by a subscriber + * + * @param msg message + */ +void Ros2CppNode::topicCallback(const std_msgs::msg::Int32& msg) { + + RCLCPP_INFO(this->get_logger(), "Message received: '%d'", msg.data); +} + + +} diff --git a/samples/ros2_cpp_lifecycle_pkg/.gitlab-ci.yml b/samples/ros2_cpp_lifecycle_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..595a118 --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_cpp_lifecycle_pkg ros2_cpp_node diff --git a/samples/ros2_cpp_lifecycle_pkg/CMakeLists.txt b/samples/ros2_cpp_lifecycle_pkg/CMakeLists.txt new file mode 100644 index 0000000..3fc8de1 --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_cpp_lifecycle_pkg) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) + +set(TARGET_NAME ros2_cpp_node) + +add_executable(${TARGET_NAME} src/ros2_cpp_node.cpp) + +target_include_directories(${TARGET_NAME} PUBLIC + $ + $ +) +target_compile_features(${TARGET_NAME} PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(${TARGET_NAME} + rclcpp + rclcpp_lifecycle + std_msgs +) + +install(TARGETS ${TARGET_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +ament_package() diff --git a/samples/ros2_cpp_lifecycle_pkg/README.md b/samples/ros2_cpp_lifecycle_pkg/README.md new file mode 100644 index 0000000..d61f6d2 --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_cpp_lifecycle_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_cpp_node](#ros2_cpp_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_cpp_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_cpp_lifecycle_pkg/config/params.yml b/samples/ros2_cpp_lifecycle_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_cpp_lifecycle_pkg/include/ros2_cpp_lifecycle_pkg/ros2_cpp_node.hpp b/samples/ros2_cpp_lifecycle_pkg/include/ros2_cpp_lifecycle_pkg/ros2_cpp_node.hpp new file mode 100644 index 0000000..1ef309e --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/include/ros2_cpp_lifecycle_pkg/ros2_cpp_node.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace ros2_cpp_lifecycle_pkg { + +template struct is_vector : std::false_type {}; +template struct is_vector< std::vector > : std::true_type {}; +template inline constexpr bool is_vector_v = is_vector::value; + + +class Ros2CppNode : public rclcpp_lifecycle::LifecycleNode { + + public: + + Ros2CppNode(); + + protected: + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override; + + private: + + template + void declareAndLoadParameter(const std::string &name, + T ¶m, + const std::string &description, + const bool add_to_auto_reconfigurable_params = true, + const bool is_required = false, + const bool read_only = false, + const std::optional &from_value = std::nullopt, + const std::optional &to_value = std::nullopt, + const std::optional &step_value = std::nullopt, + const std::string &additional_constraints = ""); + + rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector& parameters); + + void setup(); + + void cleanUp(); + + void topicCallback(const std_msgs::msg::Int32& msg); + + private: + + std::vector>> auto_reconfigurable_params_; + + OnSetParametersCallbackHandle::SharedPtr parameters_callback_; + + rclcpp::Subscription::SharedPtr subscriber_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + + double param_ = 1.0; +}; + + +} diff --git a/samples/ros2_cpp_lifecycle_pkg/launch/ros2_cpp_node_launch.py b/samples/ros2_cpp_lifecycle_pkg/launch/ros2_cpp_node_launch.py new file mode 100644 index 0000000..98ba960 --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/launch/ros2_cpp_node_launch.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LifecycleNode, SetParameter + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_cpp_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_cpp_lifecycle_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + DeclareLaunchArgument("startup_state", default_value="None", description="initial lifecycle state"), + ] + + nodes = [ + GroupAction( + actions=[ + SetParameter( + name="startup_state", + value=LaunchConfiguration("startup_state"), + condition=LaunchConfigurationNotEquals("startup_state", "None") + ), + LifecycleNode( + package="ros2_cpp_lifecycle_pkg", + executable="ros2_cpp_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_cpp_lifecycle_pkg/package.xml b/samples/ros2_cpp_lifecycle_pkg/package.xml new file mode 100644 index 0000000..569c6d0 --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/package.xml @@ -0,0 +1,24 @@ + + + + + ros2_cpp_lifecycle_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + ament_cmake + + rclcpp + rclcpp_lifecycle + std_msgs + + + ament_cmake + + + diff --git a/samples/ros2_cpp_lifecycle_pkg/src/ros2_cpp_node.cpp b/samples/ros2_cpp_lifecycle_pkg/src/ros2_cpp_node.cpp new file mode 100644 index 0000000..f0bdd4e --- /dev/null +++ b/samples/ros2_cpp_lifecycle_pkg/src/ros2_cpp_node.cpp @@ -0,0 +1,264 @@ +#include +#include + +#include + + +namespace ros2_cpp_lifecycle_pkg { + + +/** + * @brief Constructor + * + * @param options node options + */ +Ros2CppNode::Ros2CppNode() : rclcpp_lifecycle::LifecycleNode("ros2_cpp_node") { + + int startup_state = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + this->declareAndLoadParameter("startup_state", startup_state, "Initial lifecycle state", false, false, false); + if (startup_state > lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (startup_state > lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); +} + + +/** + * @brief Declares and loads a ROS parameter + * + * @param name name + * @param param parameter variable to load into + * @param description description + * @param add_to_auto_reconfigurable_params enable reconfiguration of parameter + * @param is_required whether failure to load parameter will stop node + * @param read_only set parameter to read-only + * @param from_value parameter range minimum + * @param to_value parameter range maximum + * @param step_value parameter range step + * @param additional_constraints additional constraints description + */ +template +void Ros2CppNode::declareAndLoadParameter(const std::string& name, + T& param, + const std::string& description, + const bool add_to_auto_reconfigurable_params, + const bool is_required, + const bool read_only, + const std::optional& from_value, + const std::optional& to_value, + const std::optional& step_value, + const std::string& additional_constraints) { + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = description; + param_desc.additional_constraints = additional_constraints; + param_desc.read_only = read_only; + + auto type = rclcpp::ParameterValue(param).get_type(); + + if (from_value.has_value() && to_value.has_value()) { + if constexpr(std::is_integral_v) { + rcl_interfaces::msg::IntegerRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.integer_range = {range}; + } else if constexpr(std::is_floating_point_v) { + rcl_interfaces::msg::FloatingPointRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1.0); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.floating_point_range = {range}; + } else { + RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str()); + } + } + + this->declare_parameter(name, type, param_desc); + + try { + param = this->get_parameter(name).get_value(); + std::stringstream ss; + ss << "Loaded parameter '" << name << "': "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_INFO_STREAM(this->get_logger(), ss.str()); + } catch (rclcpp::exceptions::ParameterUninitializedException&) { + if (is_required) { + RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting"); + exit(EXIT_FAILURE); + } else { + std::stringstream ss; + ss << "Missing parameter '" << name << "', using default value: "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_WARN_STREAM(this->get_logger(), ss.str()); + } + } + + if (add_to_auto_reconfigurable_params) { + std::function setter = [¶m](const rclcpp::Parameter& p) { + param = p.get_value(); + }; + auto_reconfigurable_params_.push_back(std::make_tuple(name, setter)); + } +} + + +/** + * @brief Handles reconfiguration when a parameter value is changed + * + * @param parameters parameters + * @return parameter change result + */ +rcl_interfaces::msg::SetParametersResult Ros2CppNode::parametersCallback(const std::vector& parameters) { + + for (const auto& param : parameters) { + for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) { + if (param.get_name() == std::get<0>(auto_reconfigurable_param)) { + std::get<1>(auto_reconfigurable_param)(param); + RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str()); + break; + } + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + return result; +} + + +/** + * @brief Sets up subscribers, publishers, etc. to configure the node + */ +void Ros2CppNode::setup() { + + // callback for dynamic parameter configuration + parameters_callback_ = this->add_on_set_parameters_callback(std::bind(&Ros2CppNode::parametersCallback, this, std::placeholders::_1)); + + // subscriber for handling incoming messages + subscriber_ = this->create_subscription("~/input", 10, std::bind(&Ros2CppNode::topicCallback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", subscriber_->get_topic_name()); + + // publisher for publishing outgoing messages + publisher_ = this->create_publisher("~/output", 10); + RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", publisher_->get_topic_name()); +} + + +/** + * @brief Processes messages received by a subscriber + * + * @param msg message + */ +void Ros2CppNode::topicCallback(const std_msgs::msg::Int32& msg) { + + RCLCPP_INFO(this->get_logger(), "Message received: '%d'", msg.data); +} + + +/** + * @brief Processes 'configuring' transitions to 'inactive' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_configure(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Configuring to enter 'inactive' state from '%s' state", state.label().c_str()); + + this->declareAndLoadParameter("param", param_, "TODO", true, false, false, 0.0, 10.0, 1.0); + setup(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'activating' transitions to 'active' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_activate(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Activating to enter 'active' state from '%s' state", state.label().c_str()); + + publisher_->on_activate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'deactivating' transitions to 'inactive' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_deactivate(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Deactivating to enter 'inactive' state from '%s' state", state.label().c_str()); + + publisher_->on_deactivate(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'cleningup' transitions to 'unconfigured' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_cleanup(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Cleaning up to enter 'unconfigured' state from '%s' state", state.label().c_str()); + + subscriber_.reset(); + publisher_.reset(); + parameters_callback_.reset(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +/** + * @brief Processes 'shuttingdown' transitions to 'finalized' state + * + * @param state previous state + * @return transition result + */ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2CppNode::on_shutdown(const rclcpp_lifecycle::State& state) { + + RCLCPP_INFO(get_logger(), "Shutting down to enter 'finalized' state from '%s' state", state.label().c_str()); + + if (state.id() >= lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + on_deactivate(state); + if (state.id() >= lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + on_cleanup(state); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + + +} + + +int main(int argc, char *argv[]) { + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/samples/ros2_cpp_pkg/.gitlab-ci.yml b/samples/ros2_cpp_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..3f0d862 --- /dev/null +++ b/samples/ros2_cpp_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_cpp_pkg ros2_cpp_node diff --git a/samples/ros2_cpp_pkg/CMakeLists.txt b/samples/ros2_cpp_pkg/CMakeLists.txt new file mode 100644 index 0000000..b77fc8e --- /dev/null +++ b/samples/ros2_cpp_pkg/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_cpp_pkg) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +set(TARGET_NAME ros2_cpp_node) + +add_executable(${TARGET_NAME} src/ros2_cpp_node.cpp) + +target_include_directories(${TARGET_NAME} PUBLIC + $ + $ +) +target_compile_features(${TARGET_NAME} PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(${TARGET_NAME} + rclcpp + std_msgs +) + +install(TARGETS ${TARGET_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} + OPTIONAL +) + +ament_package() diff --git a/samples/ros2_cpp_pkg/README.md b/samples/ros2_cpp_pkg/README.md new file mode 100644 index 0000000..60372bb --- /dev/null +++ b/samples/ros2_cpp_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_cpp_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_cpp_node](#ros2_cpp_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_cpp_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_cpp_pkg/config/params.yml b/samples/ros2_cpp_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_cpp_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_cpp_pkg/include/ros2_cpp_pkg/ros2_cpp_node.hpp b/samples/ros2_cpp_pkg/include/ros2_cpp_pkg/ros2_cpp_node.hpp new file mode 100644 index 0000000..4f63b85 --- /dev/null +++ b/samples/ros2_cpp_pkg/include/ros2_cpp_pkg/ros2_cpp_node.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include + +#include +#include + + +namespace ros2_cpp_pkg { + +template struct is_vector : std::false_type {}; +template struct is_vector< std::vector > : std::true_type {}; +template inline constexpr bool is_vector_v = is_vector::value; + + +class Ros2CppNode : public rclcpp::Node { + + public: + + Ros2CppNode(); + + private: + + template + void declareAndLoadParameter(const std::string &name, + T ¶m, + const std::string &description, + const bool add_to_auto_reconfigurable_params = true, + const bool is_required = false, + const bool read_only = false, + const std::optional &from_value = std::nullopt, + const std::optional &to_value = std::nullopt, + const std::optional &step_value = std::nullopt, + const std::string &additional_constraints = ""); + + rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector& parameters); + + void setup(); + + void topicCallback(const std_msgs::msg::Int32& msg); + + private: + + std::vector>> auto_reconfigurable_params_; + + OnSetParametersCallbackHandle::SharedPtr parameters_callback_; + + rclcpp::Subscription::SharedPtr subscriber_; + + rclcpp::Publisher::SharedPtr publisher_; + + double param_ = 1.0; +}; + + +} diff --git a/samples/ros2_cpp_pkg/launch/ros2_cpp_node_launch.py b/samples/ros2_cpp_pkg/launch/ros2_cpp_node_launch.py new file mode 100644 index 0000000..c270c91 --- /dev/null +++ b/samples/ros2_cpp_pkg/launch/ros2_cpp_node_launch.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_cpp_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_cpp_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + ] + + nodes = [ + Node( + package="ros2_cpp_pkg", + executable="ros2_cpp_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_cpp_pkg/package.xml b/samples/ros2_cpp_pkg/package.xml new file mode 100644 index 0000000..e9ce970 --- /dev/null +++ b/samples/ros2_cpp_pkg/package.xml @@ -0,0 +1,23 @@ + + + + + ros2_cpp_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + ament_cmake + + rclcpp + std_msgs + + + ament_cmake + + + diff --git a/samples/ros2_cpp_pkg/src/ros2_cpp_node.cpp b/samples/ros2_cpp_pkg/src/ros2_cpp_node.cpp new file mode 100644 index 0000000..15ae866 --- /dev/null +++ b/samples/ros2_cpp_pkg/src/ros2_cpp_node.cpp @@ -0,0 +1,173 @@ +#include + +#include + + +namespace ros2_cpp_pkg { + + +/** + * @brief Constructor + * + * @param options node options + */ +Ros2CppNode::Ros2CppNode() : Node("ros2_cpp_node") { + + this->declareAndLoadParameter("param", param_, "TODO", true, false, false, 0.0, 10.0, 1.0); + this->setup(); +} + + +/** + * @brief Declares and loads a ROS parameter + * + * @param name name + * @param param parameter variable to load into + * @param description description + * @param add_to_auto_reconfigurable_params enable reconfiguration of parameter + * @param is_required whether failure to load parameter will stop node + * @param read_only set parameter to read-only + * @param from_value parameter range minimum + * @param to_value parameter range maximum + * @param step_value parameter range step + * @param additional_constraints additional constraints description + */ +template +void Ros2CppNode::declareAndLoadParameter(const std::string& name, + T& param, + const std::string& description, + const bool add_to_auto_reconfigurable_params, + const bool is_required, + const bool read_only, + const std::optional& from_value, + const std::optional& to_value, + const std::optional& step_value, + const std::string& additional_constraints) { + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = description; + param_desc.additional_constraints = additional_constraints; + param_desc.read_only = read_only; + + auto type = rclcpp::ParameterValue(param).get_type(); + + if (from_value.has_value() && to_value.has_value()) { + if constexpr(std::is_integral_v) { + rcl_interfaces::msg::IntegerRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.integer_range = {range}; + } else if constexpr(std::is_floating_point_v) { + rcl_interfaces::msg::FloatingPointRange range; + T step = static_cast(step_value.has_value() ? step_value.value() : 1.0); + range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); + param_desc.floating_point_range = {range}; + } else { + RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str()); + } + } + + this->declare_parameter(name, type, param_desc); + + try { + param = this->get_parameter(name).get_value(); + std::stringstream ss; + ss << "Loaded parameter '" << name << "': "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_INFO_STREAM(this->get_logger(), ss.str()); + } catch (rclcpp::exceptions::ParameterUninitializedException&) { + if (is_required) { + RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting"); + exit(EXIT_FAILURE); + } else { + std::stringstream ss; + ss << "Missing parameter '" << name << "', using default value: "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_WARN_STREAM(this->get_logger(), ss.str()); + } + } + + if (add_to_auto_reconfigurable_params) { + std::function setter = [¶m](const rclcpp::Parameter& p) { + param = p.get_value(); + }; + auto_reconfigurable_params_.push_back(std::make_tuple(name, setter)); + } +} + + +/** + * @brief Handles reconfiguration when a parameter value is changed + * + * @param parameters parameters + * @return parameter change result + */ +rcl_interfaces::msg::SetParametersResult Ros2CppNode::parametersCallback(const std::vector& parameters) { + + for (const auto& param : parameters) { + for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) { + if (param.get_name() == std::get<0>(auto_reconfigurable_param)) { + std::get<1>(auto_reconfigurable_param)(param); + RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str()); + break; + } + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + return result; +} + + +/** + * @brief Sets up subscribers, publishers, etc. to configure the node + */ +void Ros2CppNode::setup() { + + // callback for dynamic parameter configuration + parameters_callback_ = this->add_on_set_parameters_callback(std::bind(&Ros2CppNode::parametersCallback, this, std::placeholders::_1)); + + // subscriber for handling incoming messages + subscriber_ = this->create_subscription("~/input", 10, std::bind(&Ros2CppNode::topicCallback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", subscriber_->get_topic_name()); + + // publisher for publishing outgoing messages + publisher_ = this->create_publisher("~/output", 10); + RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", publisher_->get_topic_name()); +} + + +/** + * @brief Processes messages received by a subscriber + * + * @param msg message + */ +void Ros2CppNode::topicCallback(const std_msgs::msg::Int32& msg) { + + RCLCPP_INFO(this->get_logger(), "Message received: '%d'", msg.data); +} + + +} + + +int main(int argc, char *argv[]) { + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/samples/ros2_interfaces_pkg/CMakeLists.txt b/samples/ros2_interfaces_pkg/CMakeLists.txt new file mode 100644 index 0000000..a4c1359 --- /dev/null +++ b/samples/ros2_interfaces_pkg/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_interfaces_pkg) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/Message.msg + srv/Service.srv + action/Action.action + DEPENDENCIES +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/samples/ros2_interfaces_pkg/README.md b/samples/ros2_interfaces_pkg/README.md new file mode 100644 index 0000000..7f761b3 --- /dev/null +++ b/samples/ros2_interfaces_pkg/README.md @@ -0,0 +1,22 @@ +# ros2_interfaces_pkg + +TODO + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/samples/ros2_interfaces_pkg/action/Action.action b/samples/ros2_interfaces_pkg/action/Action.action new file mode 100644 index 0000000..8e9bc7f --- /dev/null +++ b/samples/ros2_interfaces_pkg/action/Action.action @@ -0,0 +1,5 @@ +int32 goal +--- +int32 feedback +--- +int32 result \ No newline at end of file diff --git a/samples/ros2_interfaces_pkg/msg/Message.msg b/samples/ros2_interfaces_pkg/msg/Message.msg new file mode 100644 index 0000000..0ecfe35 --- /dev/null +++ b/samples/ros2_interfaces_pkg/msg/Message.msg @@ -0,0 +1 @@ +int32 data \ No newline at end of file diff --git a/samples/ros2_interfaces_pkg/package.xml b/samples/ros2_interfaces_pkg/package.xml new file mode 100644 index 0000000..2ed88d0 --- /dev/null +++ b/samples/ros2_interfaces_pkg/package.xml @@ -0,0 +1,24 @@ + + + + + ros2_interfaces_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/samples/ros2_interfaces_pkg/srv/Service.srv b/samples/ros2_interfaces_pkg/srv/Service.srv new file mode 100644 index 0000000..3ad5501 --- /dev/null +++ b/samples/ros2_interfaces_pkg/srv/Service.srv @@ -0,0 +1,3 @@ +int32 request +--- +int32 response \ No newline at end of file diff --git a/samples/ros2_python_all_pkg/.gitlab-ci.yml b/samples/ros2_python_all_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..2bd5e7c --- /dev/null +++ b/samples/ros2_python_all_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_python_all_pkg ros2_python_node diff --git a/samples/ros2_python_all_pkg/README.md b/samples/ros2_python_all_pkg/README.md new file mode 100644 index 0000000..9b349ad --- /dev/null +++ b/samples/ros2_python_all_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_python_all_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_python_node](#ros2_python_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_python_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_python_all_pkg/config/params.yml b/samples/ros2_python_all_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_python_all_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_python_all_pkg/launch/ros2_python_node_launch.py b/samples/ros2_python_all_pkg/launch/ros2_python_node_launch.py new file mode 100644 index 0000000..9d31fcf --- /dev/null +++ b/samples/ros2_python_all_pkg/launch/ros2_python_node_launch.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_python_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_python_all_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + ] + + nodes = [ + Node( + package="ros2_python_all_pkg", + executable="ros2_python_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_python_all_pkg/package.xml b/samples/ros2_python_all_pkg/package.xml new file mode 100644 index 0000000..e3bf6a9 --- /dev/null +++ b/samples/ros2_python_all_pkg/package.xml @@ -0,0 +1,23 @@ + + + + + ros2_python_all_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + rclpy + std_msgs + std_srvs + ros2_python_all_pkg_interfaces + + + ament_python + + + diff --git a/samples/ros2_python_all_pkg/resource/ros2_python_all_pkg b/samples/ros2_python_all_pkg/resource/ros2_python_all_pkg new file mode 100644 index 0000000..e69de29 diff --git a/samples/ros2_python_all_pkg/ros2_python_all_pkg/__init__.py b/samples/ros2_python_all_pkg/ros2_python_all_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/samples/ros2_python_all_pkg/ros2_python_all_pkg/ros2_python_node.py b/samples/ros2_python_all_pkg/ros2_python_all_pkg/ros2_python_node.py new file mode 100644 index 0000000..f5b1016 --- /dev/null +++ b/samples/ros2_python_all_pkg/ros2_python_all_pkg/ros2_python_node.py @@ -0,0 +1,293 @@ +import time +from typing import Any, Optional, Union + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.node import Node +from rcl_interfaces.msg import (FloatingPointRange, IntegerRange, ParameterDescriptor, SetParametersResult) +from std_msgs.msg import Int32 +from std_srvs.srv import SetBool +from ros2_python_all_pkg_interfaces.action import Fibonacci + + +class Ros2PythonNode(Node): + + def __init__(self): + """Constructor""" + + super().__init__("ros2_python_node") + + self.auto_reconfigurable_params = [] + self.parameters_callback = None + self.subscriber = None + self.publisher = None + + self.param = 1.0 + self.param = self.declareAndLoadParameter(name="param", + param_type=rclpy.Parameter.Type.DOUBLE, + description="TODO", + default=self.param, + from_value=0.0, + to_value=10.0, + step_value=0.1) + + self.setup() + + def declareAndLoadParameter(self, + name: str, + param_type: rclpy.Parameter.Type, + description: str, + default: Optional[Any] = None, + add_to_auto_reconfigurable_params: bool = True, + is_required: bool = False, + read_only: bool = False, + from_value: Optional[Union[int, float]] = None, + to_value: Optional[Union[int, float]] = None, + step_value: Optional[Union[int, float]] = None, + additional_constraints: str = "") -> Any: + """Declares and loads a ROS parameter + + Args: + name (str): name + param_type (rclpy.Parameter.Type): parameter type + description (str): description + default (Optional[Any], optional): default value + add_to_auto_reconfigurable_params (bool, optional): enable reconfiguration of parameter + is_required (bool, optional): whether failure to load parameter will stop node + read_only (bool, optional): set parameter to read-only + from_value (Optional[Union[int, float]], optional): parameter range minimum + to_value (Optional[Union[int, float]], optional): parameter range maximum + step_value (Optional[Union[int, float]], optional): parameter range step + additional_constraints (str, optional): additional constraints description + + Returns: + Any: parameter value + """ + + # declare parameter + param_desc = ParameterDescriptor() + param_desc.description = description + param_desc.additional_constraints = additional_constraints + param_desc.read_only = read_only + if from_value is not None and to_value is not None: + if param_type == rclpy.Parameter.Type.INTEGER: + step_value = step_value if step_value is not None else 1 + param_desc.integer_range = [IntegerRange(from_value=from_value, to_value=to_value, step=step_value)] + elif param_type == rclpy.Parameter.Type.DOUBLE: + step_value = step_value if step_value is not None else 1.0 + param_desc.floating_point_range = [FloatingPointRange(from_value=from_value, to_value=to_value, step=step_value)] + else: + self.get_logger().warn(f"Parameter type of parameter '{name}' does not support specifying a range") + self.declare_parameter(name, param_type, param_desc) + + # load parameter + try: + param = self.get_parameter(name).value + self.get_logger().info(f"Loaded parameter '{name}': {param}") + except rclpy.exceptions.ParameterUninitializedException: + if is_required: + self.get_logger().fatal(f"Missing required parameter '{name}', exiting") + raise SystemExit(1) + else: + self.get_logger().warn(f"Missing parameter '{name}', using default value: {default}") + param = default + + # add parameter to auto-reconfigurable parameters + if add_to_auto_reconfigurable_params: + self.auto_reconfigurable_params.append(name) + + return param + + def parametersCallback(self, + parameters: rclpy.Parameter) -> SetParametersResult: + """Handles reconfiguration when a parameter value is changed + + Args: + parameters (rclpy.Parameter): parameters + + Returns: + SetParametersResult: parameter change result + """ + + for param in parameters: + if param.name in self.auto_reconfigurable_params: + setattr(self, param.name, param.value) + self.get_logger().info(f"Reconfigured parameter '{param.name}' to: {param.value}") + + result = SetParametersResult() + result.successful = True + + return result + + def setup(self): + """Sets up subscribers, publishers, etc. to configure the node""" + + # callback for dynamic parameter configuration + self.parameters_callback = self.add_on_set_parameters_callback( + self.parametersCallback) + + # subscriber for handling incoming messages + self.subscriber = self.create_subscription(Int32, + "~/input", + self.topicCallback, + qos_profile=10) + self.get_logger().info(f"Subscribed to '{self.subscriber.topic_name}'") + + # publisher for publishing outgoing messages + self.publisher = self.create_publisher(Int32, + "~/output", + qos_profile=10) + self.get_logger().info(f"Publishing to '{self.publisher.topic_name}'") + + # service server for handling service calls + self.service_server = self.create_service(SetBool, "~/service", self.serviceCallback) + + # action server for handling action goal requests + self.action_server = ActionServer( + self, + Fibonacci, + "~/action", + execute_callback=self.actionExecute, + goal_callback=self.actionHandleGoal, + cancel_callback=self.actionHandleCancel, + handle_accepted_callback=self.actionHandleAccepted + ) + + # timer for repeatedly invoking a callback to publish messages + self.timer = self.create_timer(1.0, self.timerCallback) + + def topicCallback(self, msg: Int32): + """Processes messages received by a subscriber + + Args: + msg (Int32): message + """ + + self.get_logger().info(f"Message received: '{msg.data}'") + + def serviceCallback(self, request: SetBool.Request, response: SetBool.Response) -> SetBool.Response: + """Processes service requests + + Args: + request (SetBool.Request): service request + response (SetBool.Response): service response + + Returns: + SetBool.Response: service response + """ + + self.get_logger().info("Received service request") + response.success = True + + return response + + def actionHandleGoal(self, goal: Fibonacci.Goal) -> GoalResponse: + """Processes action goal requests + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + GoalResponse: goal response + """ + + self.get_logger().info("Received action goal request") + + return GoalResponse.ACCEPT + + def actionHandleCancel(self, goal: Fibonacci.Goal) -> CancelResponse: + """Processes action cancel requests + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + CancelResponse: cancel response + """ + + self.get_logger().info("Received request to cancel action goal") + + return CancelResponse.ACCEPT + + def actionHandleAccepted(self, goal: Fibonacci.Goal): + """Processes accepted action goal requests + + Args: + goal (Fibonacci.Goal): action goal + """ + + # execute action in a separate thread to avoid blocking + goal.execute() + + async def actionExecute(self, goal: Fibonacci.Goal) -> Fibonacci.Result: + """Executes an action + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + Fibonacci.Result: action goal result + """ + + self.get_logger().info("Executing action goal") + + # define a sleeping rate between computing individual Fibonacci numbers + loop_rate = 1 + + # create the action feedback and result + feedback = Fibonacci.Feedback() + result = Fibonacci.Result() + + # initialize the Fibonacci sequence + feedback.partial_sequence = [0, 1] + + # compute the Fibonacci sequence up to the requested order n + for i in range(1, goal.request.order): + + # cancel, if requested + if goal.is_cancel_requested: + result.sequence = feedback.partial_sequence + goal.canceled() + self.get_logger().info("Action goal canceled") + return result + + # compute the next Fibonacci number + feedback.partial_sequence.append(feedback.partial_sequence[i] + feedback.partial_sequence[i-1]) + + # publish the current sequence as action feedback + goal.publish_feedback(feedback) + self.get_logger().info("Publishing action feedback") + + # sleep before computing the next Fibonacci number + time.sleep(loop_rate) + + # finish by publishing the action result + if rclpy.ok(): + result.sequence = feedback.partial_sequence + goal.succeed() + self.get_logger().info("Goal succeeded") + + return result + + def timerCallback(self): + """Processes timer triggers + """ + + self.get_logger().info("Timer triggered") + + +def main(): + + rclpy.init() + node = Ros2PythonNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/samples/ros2_python_all_pkg/setup.cfg b/samples/ros2_python_all_pkg/setup.cfg new file mode 100644 index 0000000..1908e6c --- /dev/null +++ b/samples/ros2_python_all_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros2_python_all_pkg +[install] +install_scripts=$base/lib/ros2_python_all_pkg diff --git a/samples/ros2_python_all_pkg/setup.py b/samples/ros2_python_all_pkg/setup.py new file mode 100644 index 0000000..20fa2ab --- /dev/null +++ b/samples/ros2_python_all_pkg/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'ros2_python_all_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + (os.path.join('share', package_name), ['package.xml']), + (os.path.join('share', package_name, + 'launch'), glob('launch/*launch.[pxy][yma]*')), + (os.path.join('share', package_name, + 'config'), glob('config/*'))], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['ros2_python_node = ros2_python_all_pkg.ros2_python_node:main'], + }, +) diff --git a/samples/ros2_python_all_pkg_interfaces/CMakeLists.txt b/samples/ros2_python_all_pkg_interfaces/CMakeLists.txt new file mode 100644 index 0000000..4adc66e --- /dev/null +++ b/samples/ros2_python_all_pkg_interfaces/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_python_all_pkg_interfaces) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + action/Fibonacci.action +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/samples/ros2_python_all_pkg_interfaces/README.md b/samples/ros2_python_all_pkg_interfaces/README.md new file mode 100644 index 0000000..0c8d8f6 --- /dev/null +++ b/samples/ros2_python_all_pkg_interfaces/README.md @@ -0,0 +1,22 @@ +# ros2_python_all_pkg_interfaces + +ROS interface definitions for ros2_python_all_pkg + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/samples/ros2_python_all_pkg_interfaces/action/Fibonacci.action b/samples/ros2_python_all_pkg_interfaces/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/samples/ros2_python_all_pkg_interfaces/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/samples/ros2_python_all_pkg_interfaces/package.xml b/samples/ros2_python_all_pkg_interfaces/package.xml new file mode 100644 index 0000000..940d76e --- /dev/null +++ b/samples/ros2_python_all_pkg_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + + ros2_python_all_pkg_interfaces + 0.0.0 + ROS interface definitions for ros2_python_all_pkg + + TODO + TODO + + TODO + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/samples/ros2_python_pkg/.gitlab-ci.yml b/samples/ros2_python_pkg/.gitlab-ci.yml new file mode 100644 index 0000000..333fe08 --- /dev/null +++ b/samples/ros2_python_pkg/.gitlab-ci.yml @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run ros2_python_pkg ros2_python_node diff --git a/samples/ros2_python_pkg/README.md b/samples/ros2_python_pkg/README.md new file mode 100644 index 0000000..558c4cd --- /dev/null +++ b/samples/ros2_python_pkg/README.md @@ -0,0 +1,46 @@ +# ros2_python_pkg + +TODO + +- [Container Images](#container-images) +- [ros2_python_node](#ros2_python_node) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `ros2_python_node` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/samples/ros2_python_pkg/config/params.yml b/samples/ros2_python_pkg/config/params.yml new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/samples/ros2_python_pkg/config/params.yml @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/samples/ros2_python_pkg/launch/ros2_python_node_launch.py b/samples/ros2_python_pkg/launch/ros2_python_node_launch.py new file mode 100644 index 0000000..c4d5a5b --- /dev/null +++ b/samples/ros2_python_pkg/launch/ros2_python_node_launch.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="ros2_python_node", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("ros2_python_pkg"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + ] + + nodes = [ + Node( + package="ros2_python_pkg", + executable="ros2_python_node", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), + parameters=[LaunchConfiguration("params")], + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/samples/ros2_python_pkg/package.xml b/samples/ros2_python_pkg/package.xml new file mode 100644 index 0000000..e37ac9c --- /dev/null +++ b/samples/ros2_python_pkg/package.xml @@ -0,0 +1,21 @@ + + + + + ros2_python_pkg + 0.0.0 + TODO + + TODO + TODO + + TODO + + rclpy + std_msgs + + + ament_python + + + diff --git a/samples/ros2_python_pkg/resource/ros2_python_pkg b/samples/ros2_python_pkg/resource/ros2_python_pkg new file mode 100644 index 0000000..e69de29 diff --git a/samples/ros2_python_pkg/ros2_python_pkg/__init__.py b/samples/ros2_python_pkg/ros2_python_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/samples/ros2_python_pkg/ros2_python_pkg/ros2_python_node.py b/samples/ros2_python_pkg/ros2_python_pkg/ros2_python_node.py new file mode 100644 index 0000000..664bd43 --- /dev/null +++ b/samples/ros2_python_pkg/ros2_python_pkg/ros2_python_node.py @@ -0,0 +1,162 @@ +from typing import Any, Optional, Union + +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import (FloatingPointRange, IntegerRange, ParameterDescriptor, SetParametersResult) +from std_msgs.msg import Int32 + + +class Ros2PythonNode(Node): + + def __init__(self): + """Constructor""" + + super().__init__("ros2_python_node") + + self.auto_reconfigurable_params = [] + self.parameters_callback = None + self.subscriber = None + self.publisher = None + + self.param = 1.0 + self.param = self.declareAndLoadParameter(name="param", + param_type=rclpy.Parameter.Type.DOUBLE, + description="TODO", + default=self.param, + from_value=0.0, + to_value=10.0, + step_value=0.1) + + self.setup() + + def declareAndLoadParameter(self, + name: str, + param_type: rclpy.Parameter.Type, + description: str, + default: Optional[Any] = None, + add_to_auto_reconfigurable_params: bool = True, + is_required: bool = False, + read_only: bool = False, + from_value: Optional[Union[int, float]] = None, + to_value: Optional[Union[int, float]] = None, + step_value: Optional[Union[int, float]] = None, + additional_constraints: str = "") -> Any: + """Declares and loads a ROS parameter + + Args: + name (str): name + param_type (rclpy.Parameter.Type): parameter type + description (str): description + default (Optional[Any], optional): default value + add_to_auto_reconfigurable_params (bool, optional): enable reconfiguration of parameter + is_required (bool, optional): whether failure to load parameter will stop node + read_only (bool, optional): set parameter to read-only + from_value (Optional[Union[int, float]], optional): parameter range minimum + to_value (Optional[Union[int, float]], optional): parameter range maximum + step_value (Optional[Union[int, float]], optional): parameter range step + additional_constraints (str, optional): additional constraints description + + Returns: + Any: parameter value + """ + + # declare parameter + param_desc = ParameterDescriptor() + param_desc.description = description + param_desc.additional_constraints = additional_constraints + param_desc.read_only = read_only + if from_value is not None and to_value is not None: + if param_type == rclpy.Parameter.Type.INTEGER: + step_value = step_value if step_value is not None else 1 + param_desc.integer_range = [IntegerRange(from_value=from_value, to_value=to_value, step=step_value)] + elif param_type == rclpy.Parameter.Type.DOUBLE: + step_value = step_value if step_value is not None else 1.0 + param_desc.floating_point_range = [FloatingPointRange(from_value=from_value, to_value=to_value, step=step_value)] + else: + self.get_logger().warn(f"Parameter type of parameter '{name}' does not support specifying a range") + self.declare_parameter(name, param_type, param_desc) + + # load parameter + try: + param = self.get_parameter(name).value + self.get_logger().info(f"Loaded parameter '{name}': {param}") + except rclpy.exceptions.ParameterUninitializedException: + if is_required: + self.get_logger().fatal(f"Missing required parameter '{name}', exiting") + raise SystemExit(1) + else: + self.get_logger().warn(f"Missing parameter '{name}', using default value: {default}") + param = default + + # add parameter to auto-reconfigurable parameters + if add_to_auto_reconfigurable_params: + self.auto_reconfigurable_params.append(name) + + return param + + def parametersCallback(self, + parameters: rclpy.Parameter) -> SetParametersResult: + """Handles reconfiguration when a parameter value is changed + + Args: + parameters (rclpy.Parameter): parameters + + Returns: + SetParametersResult: parameter change result + """ + + for param in parameters: + if param.name in self.auto_reconfigurable_params: + setattr(self, param.name, param.value) + self.get_logger().info(f"Reconfigured parameter '{param.name}' to: {param.value}") + + result = SetParametersResult() + result.successful = True + + return result + + def setup(self): + """Sets up subscribers, publishers, etc. to configure the node""" + + # callback for dynamic parameter configuration + self.parameters_callback = self.add_on_set_parameters_callback( + self.parametersCallback) + + # subscriber for handling incoming messages + self.subscriber = self.create_subscription(Int32, + "~/input", + self.topicCallback, + qos_profile=10) + self.get_logger().info(f"Subscribed to '{self.subscriber.topic_name}'") + + # publisher for publishing outgoing messages + self.publisher = self.create_publisher(Int32, + "~/output", + qos_profile=10) + self.get_logger().info(f"Publishing to '{self.publisher.topic_name}'") + + def topicCallback(self, msg: Int32): + """Processes messages received by a subscriber + + Args: + msg (Int32): message + """ + + self.get_logger().info(f"Message received: '{msg.data}'") + + +def main(): + + rclpy.init() + node = Ros2PythonNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/samples/ros2_python_pkg/setup.cfg b/samples/ros2_python_pkg/setup.cfg new file mode 100644 index 0000000..6d39edb --- /dev/null +++ b/samples/ros2_python_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros2_python_pkg +[install] +install_scripts=$base/lib/ros2_python_pkg diff --git a/samples/ros2_python_pkg/setup.py b/samples/ros2_python_pkg/setup.py new file mode 100644 index 0000000..6884337 --- /dev/null +++ b/samples/ros2_python_pkg/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'ros2_python_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + (os.path.join('share', package_name), ['package.xml']), + (os.path.join('share', package_name, + 'launch'), glob('launch/*launch.[pxy][yma]*')), + (os.path.join('share', package_name, + 'config'), glob('config/*'))], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['ros2_python_node = ros2_python_pkg.ros2_python_node:main'], + }, +) diff --git a/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja index 52dbb82..d66679b 100644 --- a/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja +++ b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project({{ package_name }}_actions) +project({{ package_name }}_interfaces) find_package(rosidl_default_generators REQUIRED) diff --git a/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja new file mode 100644 index 0000000..05acf0d --- /dev/null +++ b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja @@ -0,0 +1,22 @@ +# {{ package_name }}_interfaces + +ROS interface definitions for {{ package_name }} + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja index f29b181..f7d17ba 100644 --- a/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja +++ b/templates/ros2_cpp_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja @@ -2,7 +2,7 @@ - {{ package_name }}_actions + {{ package_name }}_interfaces 0.0.0 ROS interface definitions for {{ package_name }} diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/CMakeLists.txt.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/CMakeLists.txt.jinja index 6af82ee..9093c08 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/CMakeLists.txt.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/CMakeLists.txt.jinja @@ -9,7 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) {% if has_action_server %} find_package(rclcpp_action REQUIRED) -find_package({{ package_name }}_actions REQUIRED) +find_package({{ package_name }}_interfaces REQUIRED) {% endif %} {% if is_component %} find_package(rclcpp_components REQUIRED) @@ -49,7 +49,7 @@ ament_target_dependencies(${TARGET_NAME} rclcpp {% if has_action_server %} rclcpp_action - {{ package_name }}_actions + {{ package_name }}_interfaces {% endif %} {% if is_component %} rclcpp_components diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/README.md.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/README.md.jinja index fe26b02..f95f0c2 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/README.md.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/README.md.jinja @@ -4,59 +4,43 @@ - [Container Images](#container-images) - [{{ node_name }}](#{{ node_name }}) -- [Official Documentation](#official-documentation) -## Container Images +### Container Images -| Description | Image | Command | +| Description | Image:Tag | Default Command | | --- | --- | -- | -| | ` - | Topic | Type | Description | | --- | --- | --- | -| `` | `/msg/` | \ | +| | | | ### Published Topics - - | Topic | Type | Description | | --- | --- | --- | -| `` | `/msg/` | \ | +| | | | ### Services - - | Service | Type | Description | | --- | --- | --- | -| `` | `/srv/` | \ | +| | | | ### Actions - - | Action | Type | Description | | --- | --- | --- | -| `` | `/action/` | \ | +| | | | ### Parameters - - | Parameter | Type | Description | | --- | --- | --- | -| `` | `` | \ | - - -## Official Documentation - -\ \ No newline at end of file +| | | | diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/include/{{ package_name }}/{{ node_name }}.hpp.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/include/{{ package_name }}/{{ node_name }}.hpp.jinja index 1159569..821b4f2 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/include/{{ package_name }}/{{ node_name }}.hpp.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/include/{{ package_name }}/{{ node_name }}.hpp.jinja @@ -22,7 +22,7 @@ {% endif %} {% if has_action_server %} -#include <{{ package_name }}_actions/action/fibonacci.hpp> +#include <{{ package_name }}_interfaces/action/fibonacci.hpp> {% endif %} @@ -98,13 +98,13 @@ class {{ node_class_name }} : public rclcpp::Node { {% endif %} {% if has_action_server %} - rclcpp_action::GoalResponse actionHandleGoal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + rclcpp_action::GoalResponse actionHandleGoal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse actionHandleCancel(const std::shared_ptr> goal_handle); + rclcpp_action::CancelResponse actionHandleCancel(const std::shared_ptr> goal_handle); - void actionHandleAccepted(const std::shared_ptr> goal_handle); + void actionHandleAccepted(const std::shared_ptr> goal_handle); - void actionExecute(const std::shared_ptr> goal_handle); + void actionExecute(const std::shared_ptr> goal_handle); {% endif %} {% if has_timer %} @@ -142,7 +142,7 @@ class {{ node_class_name }} : public rclcpp::Node { {% endif %} {% if has_action_server %} - rclcpp_action::Server<{{ package_name }}_actions::action::Fibonacci>::SharedPtr action_server_; + rclcpp_action::Server<{{ package_name }}_interfaces::action::Fibonacci>::SharedPtr action_server_; {% endif %} {% if has_timer %} diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/package.xml.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/package.xml.jinja index 3c08819..f5f3339 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/package.xml.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/package.xml.jinja @@ -28,7 +28,7 @@ std_srvs {% endif %} {% if has_action_server %} - {{ package_name }}_actions + {{ package_name }}_interfaces {% endif %} diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/src/{{ node_name }}.cpp.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/src/{{ node_name }}.cpp.jinja index 0048240..02ee49d 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/src/{{ node_name }}.cpp.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/src/{{ node_name }}.cpp.jinja @@ -86,18 +86,18 @@ void {{ node_class_name }}::declareAndLoadParameter(const std::string& name, auto type = rclcpp::ParameterValue(param).get_type(); if (from_value.has_value() && to_value.has_value()) { - if constexpr (std::is_integral_v) { + if constexpr(std::is_integral_v) { rcl_interfaces::msg::IntegerRange range; - T step = static_cast(step_value.has_value() ? step_value.value() : 0); + T step = static_cast(step_value.has_value() ? step_value.value() : 1); range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); param_desc.integer_range = {range}; - } else if constexpr (std::is_floating_point_v) { + } else if constexpr(std::is_floating_point_v) { rcl_interfaces::msg::FloatingPointRange range; - T step = static_cast(step_value.has_value() ? step_value.value() : 0.0); + T step = static_cast(step_value.has_value() ? step_value.value() : 1.0); range.set__from_value(static_cast(from_value.value())).set__to_value(static_cast(to_value.value())).set__step(step); param_desc.floating_point_range = {range}; } else { - RCLCPP_WARN(this->get_logger(), "Parameter type does not support specifying a range"); + RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str()); } } @@ -105,6 +105,15 @@ void {{ node_class_name }}::declareAndLoadParameter(const std::string& name, try { param = this->get_parameter(name).get_value(); + std::stringstream ss; + ss << "Loaded parameter '" << name << "': "; + if constexpr(is_vector_v) { + ss << "["; + for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); + } else { + ss << param; + } + RCLCPP_INFO_STREAM(this->get_logger(), ss.str()); } catch (rclcpp::exceptions::ParameterUninitializedException&) { if (is_required) { RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting"); @@ -112,7 +121,7 @@ void {{ node_class_name }}::declareAndLoadParameter(const std::string& name, } else { std::stringstream ss; ss << "Missing parameter '" << name << "', using default value: "; - if constexpr (is_vector_v) { + if constexpr(is_vector_v) { ss << "["; for (const auto& element : param) ss << element << (&element != ¶m.back() ? ", " : "]"); } else { @@ -145,11 +154,12 @@ rcl_interfaces::msg::SetParametersResult {{ node_class_name }}::parametersCallba for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) { if (param.get_name() == std::get<0>(auto_reconfigurable_param)) { std::get<1>(auto_reconfigurable_param)(param); + RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str()); + break; } } } - // mark parameter change successful rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -187,7 +197,7 @@ void {{ node_class_name }}::setup() { {% if has_action_server %} // action server for handling action goal requests - action_server_ = rclcpp_action::create_server<{{ package_name }}_actions::action::Fibonacci>( + action_server_ = rclcpp_action::create_server<{{ package_name }}_interfaces::action::Fibonacci>( this, "~/action", std::bind(&{{ node_class_name }}::actionHandleGoal, this, std::placeholders::_1, std::placeholders::_2), @@ -198,7 +208,7 @@ void {{ node_class_name }}::setup() { {% if has_timer and not is_lifecycle %} // timer for repeatedly invoking a callback to publish messages - timer_ = this->create_wall_timer(std::chrono::duration(param_), std::bind(&{{ node_class_name }}::timerCallback, this)); + timer_ = this->create_wall_timer(std::chrono::duration(1.0), std::bind(&{{ node_class_name }}::timerCallback, this)); {% endif %} {% if auto_shutdown and not is_lifecycle %} @@ -246,7 +256,7 @@ void {{ node_class_name }}::serviceCallback(const std_srvs::srv::SetBool::Reques * @param goal action goal * @return goal response */ -rclcpp_action::GoalResponse {{ node_class_name }}::actionHandleGoal(const rclcpp_action::GoalUUID& uuid, {{ package_name }}_actions::action::Fibonacci::Goal::ConstSharedPtr goal) { +rclcpp_action::GoalResponse {{ node_class_name }}::actionHandleGoal(const rclcpp_action::GoalUUID& uuid, {{ package_name }}_interfaces::action::Fibonacci::Goal::ConstSharedPtr goal) { (void)uuid; (void)goal; @@ -263,7 +273,7 @@ rclcpp_action::GoalResponse {{ node_class_name }}::actionHandleGoal(const rclcpp * @param goal_handle action goal handle * @return cancel response */ -rclcpp_action::CancelResponse {{ node_class_name }}::actionHandleCancel(const std::shared_ptr> goal_handle) { +rclcpp_action::CancelResponse {{ node_class_name }}::actionHandleCancel(const std::shared_ptr> goal_handle) { (void)goal_handle; @@ -278,7 +288,7 @@ rclcpp_action::CancelResponse {{ node_class_name }}::actionHandleCancel(const st * * @param goal_handle action goal handle */ -void {{ node_class_name }}::actionHandleAccepted(const std::shared_ptr> goal_handle) { +void {{ node_class_name }}::actionHandleAccepted(const std::shared_ptr> goal_handle) { // execute action in a separate thread to avoid blocking std::thread{std::bind(&{{ node_class_name }}::actionExecute, this, std::placeholders::_1), goal_handle}.detach(); @@ -290,7 +300,7 @@ void {{ node_class_name }}::actionHandleAccepted(const std::shared_ptr> goal_handle) { +void {{ node_class_name }}::actionExecute(const std::shared_ptr> goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing action goal"); @@ -299,8 +309,8 @@ void {{ node_class_name }}::actionExecute(const std::shared_ptrget_goal(); - auto feedback = std::make_shared<{{ package_name }}_actions::action::Fibonacci::Feedback>(); - auto result = std::make_shared<{{ package_name }}_actions::action::Fibonacci::Result>(); + auto feedback = std::make_shared<{{ package_name }}_interfaces::action::Fibonacci::Feedback>(); + auto result = std::make_shared<{{ package_name }}_interfaces::action::Fibonacci::Result>(); // initialize the Fibonacci sequence auto& partial_sequence = feedback->partial_sequence; diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja index 3a398d2..98e5dac 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja @@ -18,52 +18,62 @@ from launch_ros.actions import Node def generate_launch_description(): - arg_name = DeclareLaunchArgument("name", default_value="{{ node_name }}", description="node name") - arg_namespace = DeclareLaunchArgument("namespace", default_value="", description="node namespace") + args = [ + DeclareLaunchArgument("name", default_value="{{ node_name }}", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("{{ package_name }}"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), {% if is_lifecycle %} - arg_startup_state = DeclareLaunchArgument("startup_state", default_value="None", description="initial lifecycle state") + DeclareLaunchArgument("startup_state", default_value="None", description="initial lifecycle state"), {% endif %} + ] {% if is_lifecycle %} - node = LifecycleNode( + nodes = [ + GroupAction( + actions=[ + SetParameter( + name="startup_state", + value=LaunchConfiguration("startup_state"), + condition=LaunchConfigurationNotEquals("startup_state", "None") + ), + LifecycleNode( + package="{{ package_name }}", + executable="{{ node_name }}", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), +{% if has_params %} + parameters=[LaunchConfiguration("params")], {% else %} - node = Node( + parameters=[], {% endif %} - package="{{ package_name }}", - executable="{{ node_name }}", - namespace=LaunchConfiguration("namespace"), - name=LaunchConfiguration("name"), + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + ) + ] +{% else %} + nodes = [ + Node( + package="{{ package_name }}", + executable="{{ node_name }}", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), {% if has_params %} - parameters=[ - os.path.join(get_package_share_directory("{{ package_name }}"), "config", "params.yml"), - ], + parameters=[LaunchConfiguration("params")], +{% else %} + parameters=[], {% endif %} - output="screen", - emulate_tty=True, - ) -{% if is_lifecycle %} - - node_with_startup_state = GroupAction( - actions=[ - SetParameter( - name="startup_state", - value=LaunchConfiguration("startup_state"), - condition=LaunchConfigurationNotEquals("startup_state", "None") - ), - node - ] - ) + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] {% endif %} return LaunchDescription([ - arg_name, - arg_namespace, -{% if is_lifecycle %} - arg_startup_state, -{% endif %} -{% if is_lifecycle %} - node_with_startup_state, -{% else %} - node -{% endif %} + *args, + *nodes, ]) diff --git a/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja b/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja index 745aea4..5a80568 100644 --- a/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja +++ b/templates/ros2_cpp_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja @@ -2,10 +2,11 @@ + {% if has_params %} - + {% endif %} diff --git a/templates/ros2_interfaces_pkg/{{ package_name }}/README.md.jinja b/templates/ros2_interfaces_pkg/{{ package_name }}/README.md.jinja new file mode 100644 index 0000000..0dab93b --- /dev/null +++ b/templates/ros2_interfaces_pkg/{{ package_name }}/README.md.jinja @@ -0,0 +1,22 @@ +# {{ package_name }} + +{{ description }} + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja new file mode 100644 index 0000000..d66679b --- /dev/null +++ b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/CMakeLists.txt.jinja @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project({{ package_name }}_interfaces) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + action/Fibonacci.action +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja new file mode 100644 index 0000000..05acf0d --- /dev/null +++ b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/README.md.jinja @@ -0,0 +1,22 @@ +# {{ package_name }}_interfaces + +ROS interface definitions for {{ package_name }} + + +### Messages + +| Type | Description | +| --- | --- | +| | | + +### Services + +| Type | Description | +| --- | --- | +| | | + +### Actions + +| Type | Description | +| --- | --- | +| | | diff --git a/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/action/Fibonacci.action b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja new file mode 100644 index 0000000..f7d17ba --- /dev/null +++ b/templates/ros2_python_pkg/{% if has_action_server %}{{ package_name }}_interfaces{% endif %}/package.xml.jinja @@ -0,0 +1,24 @@ + + + + + {{ package_name }}_interfaces + 0.0.0 + ROS interface definitions for {{ package_name }} + + {{ maintainer }} + {{ author }} + + {{ license }} + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/templates/ros2_python_pkg/{{ package_name }}/README.md.jinja b/templates/ros2_python_pkg/{{ package_name }}/README.md.jinja new file mode 100644 index 0000000..f95f0c2 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/README.md.jinja @@ -0,0 +1,46 @@ +# {{ package_name }} + +{{ description }} + +- [Container Images](#container-images) +- [{{ node_name }}](#{{ node_name }}) + + +### Container Images + +| Description | Image:Tag | Default Command | +| --- | --- | -- | +| | | | + + +## `{{ node_name }}` + +### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| | | | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| | | | + +### Actions + +| Action | Type | Description | +| --- | --- | --- | +| | | | + +### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| | | | diff --git a/templates/ros2_python_pkg/{{ package_name }}/package.xml.jinja b/templates/ros2_python_pkg/{{ package_name }}/package.xml.jinja new file mode 100644 index 0000000..6c08f14 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/package.xml.jinja @@ -0,0 +1,27 @@ + + + + + {{ package_name }} + 0.0.0 + {{ description }} + + {{ maintainer }} + {{ author }} + + {{ license }} + + rclpy + std_msgs +{% if has_service_server %} + std_srvs +{% endif %} +{% if has_action_server %} + {{ package_name }}_interfaces +{% endif %} + + + ament_python + + + diff --git a/templates/ros2_python_pkg/{{ package_name }}/resource/{{ package_name }}.jinja b/templates/ros2_python_pkg/{{ package_name }}/resource/{{ package_name }}.jinja new file mode 100644 index 0000000..e69de29 diff --git a/templates/ros2_python_pkg/{{ package_name }}/setup.cfg.jinja b/templates/ros2_python_pkg/{{ package_name }}/setup.cfg.jinja new file mode 100644 index 0000000..e35a7b9 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/setup.cfg.jinja @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/{{ package_name }} +[install] +install_scripts=$base/lib/{{ package_name }} diff --git a/templates/ros2_python_pkg/{{ package_name }}/setup.py.jinja b/templates/ros2_python_pkg/{{ package_name }}/setup.py.jinja new file mode 100644 index 0000000..6b068c1 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/setup.py.jinja @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import setup + +package_name = '{{ package_name }}' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + (os.path.join('share', package_name), ['package.xml']), + (os.path.join('share', package_name, + 'launch'), glob('launch/*launch.[pxy][yma]*')), + (os.path.join('share', package_name, + 'config'), glob('config/*'))], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['{{ node_name }} = {{ package_name }}.{{ node_name }}:main'], + }, +) diff --git a/templates/ros2_python_pkg/{{ package_name }}/{% if has_docker_ros %}.gitlab-ci.yml{% endif %}.jinja b/templates/ros2_python_pkg/{{ package_name }}/{% if has_docker_ros %}.gitlab-ci.yml{% endif %}.jinja new file mode 100644 index 0000000..c01306a --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{% if has_docker_ros %}.gitlab-ci.yml{% endif %}.jinja @@ -0,0 +1,8 @@ +include: + - remote: https://raw.githubusercontent.com/ika-rwth-aachen/docker-ros/main/.gitlab-ci/docker-ros.yml + +variables: + PLATFORM: amd64,arm64 + TARGET: dev,run + BASE_IMAGE: rwthika/ros2:latest + COMMAND: ros2 run {{ package_name }} {{ node_name }} diff --git a/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja new file mode 100644 index 0000000..59ad2ac --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'py' %}{{ node_name }}_launch.py{% endif %}.jinja @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + args = [ + DeclareLaunchArgument("name", default_value="{{ node_name }}", description="node name"), + DeclareLaunchArgument("namespace", default_value="", description="node namespace"), + DeclareLaunchArgument("params", default_value=os.path.join(get_package_share_directory("{{ package_name }}"), "config", "params.yml"), description="path to parameter file"), + DeclareLaunchArgument("log_level", default_value="info", description="ROS logging level (debug, info, warn, error, fatal)"), + ] + + nodes = [ + Node( + package="{{ package_name }}", + executable="{{ node_name }}", + namespace=LaunchConfiguration("namespace"), + name=LaunchConfiguration("name"), +{% if has_params %} + parameters=[LaunchConfiguration("params")], +{% else %} + parameters=[], +{% endif %} + arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], + output="screen", + emulate_tty=True, + ) + ] + + return LaunchDescription([ + *args, + *nodes, + ]) diff --git a/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja new file mode 100644 index 0000000..5a80568 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'xml' %}{{ node_name }}_launch.xml{% endif %}.jinja @@ -0,0 +1,13 @@ + + + + + + + +{% if has_params %} + +{% endif %} + + + diff --git a/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'yml' %}{{ node_name }}_launch.yml{% endif %}.jinja b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'yml' %}{{ node_name }}_launch.yml{% endif %}.jinja new file mode 100644 index 0000000..3216122 --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{% if has_launch_file %}launch{% endif %}/{% if launch_file_type == 'yml' %}{{ node_name }}_launch.yml{% endif %}.jinja @@ -0,0 +1,20 @@ +launch: + - arg: + name: name + default: {{ node_name }} + description: node name + - arg: + name: namespace + default: "" + description: node namespace + - node: + pkg: {{ package_name }} + exec: {{ node_name }} + namespace: "$(var namespace)" + name: "$(var name)" + output: screen +{% if has_params %} + param: + - name: param + value: 1.0 +{% endif %} diff --git a/templates/ros2_python_pkg/{{ package_name }}/{% if has_params %}config{% endif %}/params.yml.jinja b/templates/ros2_python_pkg/{{ package_name }}/{% if has_params %}config{% endif %}/params.yml.jinja new file mode 100644 index 0000000..809c91b --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{% if has_params %}config{% endif %}/params.yml.jinja @@ -0,0 +1,3 @@ +/**/*: + ros__parameters: + param: 1.0 \ No newline at end of file diff --git a/templates/ros2_python_pkg/{{ package_name }}/{{ package_name }}/__init__.py b/templates/ros2_python_pkg/{{ package_name }}/{{ package_name }}/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/templates/ros2_python_pkg/{{ package_name }}/{{ package_name }}/{{ node_name }}.py.jinja b/templates/ros2_python_pkg/{{ package_name }}/{{ package_name }}/{{ node_name }}.py.jinja new file mode 100644 index 0000000..784b2ef --- /dev/null +++ b/templates/ros2_python_pkg/{{ package_name }}/{{ package_name }}/{{ node_name }}.py.jinja @@ -0,0 +1,344 @@ +{% if has_action_server %} +import time +{% endif %} +from typing import Any, Optional, Union + +import rclpy +{% if has_action_server %} +from rclpy.action import ActionServer, CancelResponse, GoalResponse +{% endif %} +from rclpy.node import Node +{% if has_params %} +from rcl_interfaces.msg import (FloatingPointRange, IntegerRange, ParameterDescriptor, SetParametersResult) +{% endif %} +{% if has_subscriber or has_publisher %} +from std_msgs.msg import Int32 +{% endif %} +{% if has_service_server %} +from std_srvs.srv import SetBool +{% endif %} +{% if has_action_server %} +from {{ package_name }}_interfaces.action import Fibonacci +{% endif %} + + +class {{ node_class_name }}(Node): + + def __init__(self): + """Constructor""" + + super().__init__("{{ node_name }}") + +{% if has_params %} + self.auto_reconfigurable_params = [] + self.parameters_callback = None +{% endif %} + self.subscriber = None + self.publisher = None +{% if has_params %} + + self.param = 1.0 + self.param = self.declareAndLoadParameter(name="param", + param_type=rclpy.Parameter.Type.DOUBLE, + description="TODO", + default=self.param, + from_value=0.0, + to_value=10.0, + step_value=0.1) +{% endif %} + + self.setup() +{% if has_params %} + + def declareAndLoadParameter(self, + name: str, + param_type: rclpy.Parameter.Type, + description: str, + default: Optional[Any] = None, + add_to_auto_reconfigurable_params: bool = True, + is_required: bool = False, + read_only: bool = False, + from_value: Optional[Union[int, float]] = None, + to_value: Optional[Union[int, float]] = None, + step_value: Optional[Union[int, float]] = None, + additional_constraints: str = "") -> Any: + """Declares and loads a ROS parameter + + Args: + name (str): name + param_type (rclpy.Parameter.Type): parameter type + description (str): description + default (Optional[Any], optional): default value + add_to_auto_reconfigurable_params (bool, optional): enable reconfiguration of parameter + is_required (bool, optional): whether failure to load parameter will stop node + read_only (bool, optional): set parameter to read-only + from_value (Optional[Union[int, float]], optional): parameter range minimum + to_value (Optional[Union[int, float]], optional): parameter range maximum + step_value (Optional[Union[int, float]], optional): parameter range step + additional_constraints (str, optional): additional constraints description + + Returns: + Any: parameter value + """ + + # declare parameter + param_desc = ParameterDescriptor() + param_desc.description = description + param_desc.additional_constraints = additional_constraints + param_desc.read_only = read_only + if from_value is not None and to_value is not None: + if param_type == rclpy.Parameter.Type.INTEGER: + step_value = step_value if step_value is not None else 1 + param_desc.integer_range = [IntegerRange(from_value=from_value, to_value=to_value, step=step_value)] + elif param_type == rclpy.Parameter.Type.DOUBLE: + step_value = step_value if step_value is not None else 1.0 + param_desc.floating_point_range = [FloatingPointRange(from_value=from_value, to_value=to_value, step=step_value)] + else: + self.get_logger().warn(f"Parameter type of parameter '{name}' does not support specifying a range") + self.declare_parameter(name, param_type, param_desc) + + # load parameter + try: + param = self.get_parameter(name).value + self.get_logger().info(f"Loaded parameter '{name}': {param}") + except rclpy.exceptions.ParameterUninitializedException: + if is_required: + self.get_logger().fatal(f"Missing required parameter '{name}', exiting") + raise SystemExit(1) + else: + self.get_logger().warn(f"Missing parameter '{name}', using default value: {default}") + param = default + + # add parameter to auto-reconfigurable parameters + if add_to_auto_reconfigurable_params: + self.auto_reconfigurable_params.append(name) + + return param + + def parametersCallback(self, + parameters: rclpy.Parameter) -> SetParametersResult: + """Handles reconfiguration when a parameter value is changed + + Args: + parameters (rclpy.Parameter): parameters + + Returns: + SetParametersResult: parameter change result + """ + + for param in parameters: + if param.name in self.auto_reconfigurable_params: + setattr(self, param.name, param.value) + self.get_logger().info(f"Reconfigured parameter '{param.name}' to: {param.value}") + + result = SetParametersResult() + result.successful = True + + return result +{% endif %} + + def setup(self): + """Sets up subscribers, publishers, etc. to configure the node""" +{% if has_params %} + + # callback for dynamic parameter configuration + self.parameters_callback = self.add_on_set_parameters_callback( + self.parametersCallback) +{% endif %} +{% if has_subscriber %} + + # subscriber for handling incoming messages + self.subscriber = self.create_subscription(Int32, + "~/input", + self.topicCallback, + qos_profile=10) + self.get_logger().info(f"Subscribed to '{self.subscriber.topic_name}'") +{% endif %} +{% if has_publisher %} + + # publisher for publishing outgoing messages + self.publisher = self.create_publisher(Int32, + "~/output", + qos_profile=10) + self.get_logger().info(f"Publishing to '{self.publisher.topic_name}'") +{% endif %} +{% if has_service_server %} + + # service server for handling service calls + self.service_server = self.create_service(SetBool, "~/service", self.serviceCallback) +{% endif %} +{% if has_action_server %} + + # action server for handling action goal requests + self.action_server = ActionServer( + self, + Fibonacci, + "~/action", + execute_callback=self.actionExecute, + goal_callback=self.actionHandleGoal, + cancel_callback=self.actionHandleCancel, + handle_accepted_callback=self.actionHandleAccepted + ) +{% endif %} +{% if has_timer %} + + # timer for repeatedly invoking a callback to publish messages + self.timer = self.create_timer(1.0, self.timerCallback) +{% endif %} +{% if auto_shutdown %} + + self.auto_shutdown_timer = self.create_timer(3.0, self.autoShutdownTimerCallback) +{% endif %} +{% if has_subscriber %} + + def topicCallback(self, msg: Int32): + """Processes messages received by a subscriber + + Args: + msg (Int32): message + """ + + self.get_logger().info(f"Message received: '{msg.data}'") +{% endif %} +{% if has_service_server %} + + def serviceCallback(self, request: SetBool.Request, response: SetBool.Response) -> SetBool.Response: + """Processes service requests + + Args: + request (SetBool.Request): service request + response (SetBool.Response): service response + + Returns: + SetBool.Response: service response + """ + + self.get_logger().info("Received service request") + response.success = True + + return response +{% endif %} +{% if has_action_server %} + + def actionHandleGoal(self, goal: Fibonacci.Goal) -> GoalResponse: + """Processes action goal requests + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + GoalResponse: goal response + """ + + self.get_logger().info("Received action goal request") + + return GoalResponse.ACCEPT + + def actionHandleCancel(self, goal: Fibonacci.Goal) -> CancelResponse: + """Processes action cancel requests + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + CancelResponse: cancel response + """ + + self.get_logger().info("Received request to cancel action goal") + + return CancelResponse.ACCEPT + + def actionHandleAccepted(self, goal: Fibonacci.Goal): + """Processes accepted action goal requests + + Args: + goal (Fibonacci.Goal): action goal + """ + + # execute action in a separate thread to avoid blocking + goal.execute() + + async def actionExecute(self, goal: Fibonacci.Goal) -> Fibonacci.Result: + """Executes an action + + Args: + goal (Fibonacci.Goal): action goal + + Returns: + Fibonacci.Result: action goal result + """ + + self.get_logger().info("Executing action goal") + + # define a sleeping rate between computing individual Fibonacci numbers + loop_rate = 1 + + # create the action feedback and result + feedback = Fibonacci.Feedback() + result = Fibonacci.Result() + + # initialize the Fibonacci sequence + feedback.partial_sequence = [0, 1] + + # compute the Fibonacci sequence up to the requested order n + for i in range(1, goal.request.order): + + # cancel, if requested + if goal.is_cancel_requested: + result.sequence = feedback.partial_sequence + goal.canceled() + self.get_logger().info("Action goal canceled") + return result + + # compute the next Fibonacci number + feedback.partial_sequence.append(feedback.partial_sequence[i] + feedback.partial_sequence[i-1]) + + # publish the current sequence as action feedback + goal.publish_feedback(feedback) + self.get_logger().info("Publishing action feedback") + + # sleep before computing the next Fibonacci number + time.sleep(loop_rate) + + # finish by publishing the action result + if rclpy.ok(): + result.sequence = feedback.partial_sequence + goal.succeed() + self.get_logger().info("Goal succeeded") + + return result +{% endif %} +{% if has_timer %} + + def timerCallback(self): + """Processes timer triggers + """ + + self.get_logger().info("Timer triggered") +{% endif %} +{% if auto_shutdown %} + + def autoShutdownTimerCallback(self): + """Processes timer triggers to auto-shutdown the node + """ + + self.get_logger().info("Shutting down") + raise SystemExit(0) +{% endif %} + + +def main(): + + rclpy.init() + node = {{ node_class_name }}() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main()