Repository: ros-industrial/ros2_canopen Branch: master Commit: ace7db77c935 Files: 340 Total size: 1.5 MB Directory structure: gitextract_xhwsmvzo/ ├── .clang-format ├── .codespellignore ├── .github/ │ ├── ISSUE_TEMPLATE/ │ │ ├── bug_report.md │ │ ├── feature_request.md │ │ └── question.md │ ├── dependabot.yaml │ └── workflows/ │ ├── ci-format.yml │ ├── ci-ros-lint.yml.backup │ ├── docker.yml │ ├── humble.yml │ ├── humble_documentation.yml │ ├── prerelease-rolling.yml │ ├── rolling.yml │ └── rolling_documentation.yml ├── .gitignore ├── .pre-commit-config.yaml ├── Dockerfile ├── README.md ├── canopen/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── doxygen/ │ │ └── Doxyfile │ ├── package.xml │ └── sphinx/ │ ├── Makefile │ ├── _static/ │ │ └── css/ │ │ └── custom.css │ ├── application/ │ │ ├── prbt_robot.rst │ │ └── trinamic.rst │ ├── conf.py │ ├── developers-guide/ │ │ ├── architecture.rst │ │ ├── design-objectives.rst │ │ ├── new-device-manager.rst │ │ ├── new-driver.rst │ │ ├── new-master.rst │ │ └── overview.rst │ ├── index.rst │ ├── make.bat │ ├── quickstart/ │ │ ├── examples.rst │ │ ├── installation.rst │ │ └── operation.rst │ ├── software-tests/ │ │ ├── proxy-driver-test.rst │ │ └── ros2_control_system-test.rst │ └── user-guide/ │ ├── cia402-driver.rst │ ├── configuration.rst │ ├── how-to-create-a-cia301-system.rst │ ├── how-to-create-a-configuration.rst │ ├── how-to-create-a-robot-system.rst │ ├── master.rst │ ├── operation/ │ │ ├── managed-service-interface.rst │ │ ├── ros2-control-interface.rst │ │ └── service-interface.rst │ ├── operation.rst │ └── proxy-driver.rst ├── canopen_402_driver/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include/ │ │ └── canopen_402_driver/ │ │ ├── base.hpp │ │ ├── cia402_driver.hpp │ │ ├── command.hpp │ │ ├── default_homing_mode.hpp │ │ ├── homing_mode.hpp │ │ ├── lifecycle_cia402_driver.hpp │ │ ├── mode.hpp │ │ ├── mode_forward_helper.hpp │ │ ├── mode_target_helper.hpp │ │ ├── motor.hpp │ │ ├── node_interfaces/ │ │ │ ├── node_canopen_402_driver.hpp │ │ │ └── node_canopen_402_driver_impl.hpp │ │ ├── profiled_position_mode.hpp │ │ ├── state.hpp │ │ ├── visibility_control.h │ │ └── word_accessor.hpp │ ├── package.xml │ ├── src/ │ │ ├── cia402_driver.cpp │ │ ├── command.cpp │ │ ├── default_homing_mode.cpp │ │ ├── lifecycle_cia402_driver.cpp │ │ ├── motor.cpp │ │ ├── node_interfaces/ │ │ │ └── node_canopen_402_driver.cpp │ │ └── state.cpp │ └── test/ │ ├── CMakeLists.txt │ ├── master.dcf │ └── test_driver_component.cpp ├── canopen_base_driver/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include/ │ │ └── canopen_base_driver/ │ │ ├── base_driver.hpp │ │ ├── diagnostic_collector.hpp │ │ ├── lely_driver_bridge.hpp │ │ ├── lifecycle_base_driver.hpp │ │ ├── node_interfaces/ │ │ │ ├── node_canopen_base_driver.hpp │ │ │ └── node_canopen_base_driver_impl.hpp │ │ └── visibility_control.h │ ├── package.xml │ ├── src/ │ │ ├── base_driver.cpp │ │ ├── lely_driver_bridge.cpp │ │ ├── lifecycle_base_driver.cpp │ │ └── node_interfaces/ │ │ └── node_canopen_base_driver.cpp │ └── test/ │ ├── CMakeLists.txt │ ├── master.dcf │ ├── test_base_driver_component.cpp │ └── test_node_canopen_base_driver_ros.cpp ├── canopen_core/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── ConfigExtras.cmake │ ├── LICENSE │ ├── include/ │ │ └── canopen_core/ │ │ ├── configuration_manager.hpp │ │ ├── device_container.hpp │ │ ├── device_container_error.hpp │ │ ├── driver_error.hpp │ │ ├── driver_node.hpp │ │ ├── exchange.hpp │ │ ├── lifecycle_manager.hpp │ │ ├── master_error.hpp │ │ ├── master_node.hpp │ │ ├── node_interfaces/ │ │ │ ├── node_canopen_driver.hpp │ │ │ ├── node_canopen_driver_interface.hpp │ │ │ ├── node_canopen_master.hpp │ │ │ └── node_canopen_master_interface.hpp │ │ └── visibility_control.h │ ├── launch/ │ │ └── canopen.launch.py │ ├── package.xml │ ├── readme.md │ ├── scripts/ │ │ └── setup_vcan.sh │ ├── src/ │ │ ├── configuration_manager.cpp │ │ ├── device_container.cpp │ │ ├── device_container_error.cpp │ │ ├── device_container_node.cpp │ │ ├── driver_error.cpp │ │ ├── driver_node.cpp │ │ ├── lifecycle_manager.cpp │ │ ├── master_error.cpp │ │ ├── master_node.cpp │ │ └── node_interfaces/ │ │ ├── node_canopen_driver.cpp │ │ └── node_canopen_master.cpp │ └── test/ │ ├── CMakeLists.txt │ ├── bus_configs/ │ │ ├── bad_driver_duplicate.yml │ │ ├── bad_driver_no_driver.yml │ │ ├── bad_driver_no_id.yml │ │ ├── bad_driver_no_package.yml │ │ ├── bad_master_no_driver.yml │ │ ├── bad_master_no_id.yml │ │ ├── bad_master_no_package.yml │ │ ├── bad_no_master.yml │ │ ├── good_driver.yml │ │ ├── good_master.yml │ │ └── good_master_and_two_driver.yml │ ├── master.dcf │ ├── simple.eds │ ├── test_canopen_driver.cpp │ ├── test_canopen_master.cpp │ ├── test_device_container.cpp │ ├── test_errors.cpp │ ├── test_lifecycle_canopen_driver.cpp │ ├── test_lifecycle_canopen_master.cpp │ ├── test_lifecycle_manager.cpp │ ├── test_node_canopen_driver.cpp │ └── test_node_canopen_master.cpp ├── canopen_fake_slaves/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── Readme.md │ ├── config/ │ │ ├── cia402_slave.eds │ │ └── simple_slave.eds │ ├── include/ │ │ └── canopen_fake_slaves/ │ │ ├── base_slave.hpp │ │ ├── basic_slave.hpp │ │ ├── cia402_slave.hpp │ │ └── motion_generator.hpp │ ├── launch/ │ │ ├── basic_slave.launch.py │ │ └── cia402_slave.launch.py │ ├── package.xml │ └── src/ │ ├── basic_slave.cpp │ ├── cia402_slave.cpp │ └── motion_generator.cpp ├── canopen_interfaces/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── msg/ │ │ └── COData.msg │ ├── package.xml │ └── srv/ │ ├── COHeartbeatID.srv │ ├── CONmtID.srv │ ├── CONode.srv │ ├── CORead.srv │ ├── COReadID.srv │ ├── COTargetDouble.srv │ ├── COWrite.srv │ └── COWriteID.srv ├── canopen_master_driver/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include/ │ │ └── canopen_master_driver/ │ │ ├── lely_master_bridge.hpp │ │ ├── lifecycle_master_driver.hpp │ │ ├── master_driver.hpp │ │ ├── node_interfaces/ │ │ │ ├── node_canopen_basic_master.hpp │ │ │ └── node_canopen_basic_master_impl.hpp │ │ └── visibility_control.h │ ├── package.xml │ ├── src/ │ │ ├── lely_master_bridge.cpp │ │ ├── lifecycle_master_driver.cpp │ │ ├── master_driver.cpp │ │ └── node_interfaces/ │ │ └── node_canopen_basic_master.cpp │ └── test/ │ ├── CMakeLists.txt │ ├── master.dcf │ ├── test_master_driver_component.cpp │ ├── test_node_canopen_basic_master.cpp │ └── test_node_canopen_basic_master_ros.cpp ├── canopen_proxy_driver/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── config/ │ │ ├── concurrency_test/ │ │ │ └── master.dcf │ │ ├── nmt_test/ │ │ │ └── master.dcf │ │ ├── pdo_test/ │ │ │ └── master.dcf │ │ └── sdo_test/ │ │ └── master.dcf │ ├── include/ │ │ └── canopen_proxy_driver/ │ │ ├── lifecycle_proxy_driver.hpp │ │ ├── node_interfaces/ │ │ │ ├── node_canopen_proxy_driver.hpp │ │ │ └── node_canopen_proxy_driver_impl.hpp │ │ ├── proxy_driver.hpp │ │ └── visibility_control.h │ ├── package.xml │ ├── readme.md │ ├── src/ │ │ ├── lifecycle_proxy_driver.cpp │ │ ├── node_interfaces/ │ │ │ └── node_canopen_proxy_driver.cpp │ │ └── proxy_driver.cpp │ └── test/ │ ├── CMakeLists.txt │ ├── master.dcf │ ├── test_driver_component.cpp │ └── test_node_interface.cpp ├── canopen_ros2_control/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── canopen_ros2_control.xml │ ├── include/ │ │ └── canopen_ros2_control/ │ │ ├── canopen_system.hpp │ │ ├── cia402_data.hpp │ │ ├── cia402_system.hpp │ │ ├── helpers.hpp │ │ ├── robot_system.hpp │ │ └── visibility_control.h │ ├── package.xml │ ├── src/ │ │ ├── canopen_system.cpp │ │ ├── cia402_system.cpp │ │ └── robot_system.cpp │ └── test/ │ └── test_canopen_system.cpp ├── canopen_ros2_controllers/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── canopen_ros2_controllers.xml │ ├── include/ │ │ └── canopen_ros2_controllers/ │ │ ├── canopen_proxy_controller.hpp │ │ ├── cia402_device_controller.hpp │ │ └── visibility_control.h │ ├── package.xml │ ├── src/ │ │ ├── canopen_proxy_controller.cpp │ │ └── cia402_device_controller.cpp │ └── test/ │ ├── test_canopen_proxy_controller.cpp │ ├── test_canopen_proxy_controller.hpp │ ├── test_load_canopen_proxy_controller.cpp │ ├── test_load_cia402_device_controller.cpp │ └── test_load_cia402_robot_controller.cpp ├── canopen_tests/ │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── ROS_NAMESPACES.md │ ├── config/ │ │ ├── canopen_system/ │ │ │ ├── bus.yml │ │ │ ├── ros2_controllers.yaml │ │ │ └── simple.eds │ │ ├── cia402/ │ │ │ ├── bus.yml │ │ │ └── cia402_slave.eds │ │ ├── cia402_diagnostics/ │ │ │ ├── bus.yml │ │ │ └── cia402_slave.eds │ │ ├── cia402_lifecycle/ │ │ │ ├── bus.yml │ │ │ └── cia402_slave.eds │ │ ├── cia402_multichannel_system/ │ │ │ ├── README.md │ │ │ ├── bus.yml │ │ │ └── ros2_controllers.yaml │ │ ├── cia402_namespaced_system/ │ │ │ ├── bus.yml │ │ │ ├── cia402_slave.eds │ │ │ └── ros2_controllers.yaml │ │ ├── cia402_system/ │ │ │ ├── bus.yml │ │ │ ├── cia402_slave.eds │ │ │ └── ros2_controllers.yaml │ │ ├── robot_control/ │ │ │ ├── bus.yml │ │ │ ├── cia402_slave.eds │ │ │ ├── prbt_0_1.dcf │ │ │ └── ros2_controllers.yaml │ │ ├── simple/ │ │ │ ├── bus.yml │ │ │ └── simple.eds │ │ ├── simple_diagnostics/ │ │ │ ├── bus.yml │ │ │ └── simple.eds │ │ └── simple_lifecycle/ │ │ ├── bus.yml │ │ └── simple.eds │ ├── launch/ │ │ ├── analyzers/ │ │ │ ├── cia402_diagnostic_analyzer.yaml │ │ │ └── proxy_diagnostic_analyzer.yaml │ │ ├── canopen_system.launch.py │ │ ├── cia402_diagnostics_setup.launch.py │ │ ├── cia402_lifecycle_setup.launch.py │ │ ├── cia402_namespaced_system.launch.py │ │ ├── cia402_setup.launch.py │ │ ├── cia402_system.launch.py │ │ ├── proxy_diagnostics_setup.launch.py │ │ ├── proxy_lifecycle_setup.launch.py │ │ ├── proxy_setup.launch.py │ │ ├── proxy_setup_namespaced.launch.py │ │ ├── robot_control_setup.launch.py │ │ └── view_urdf.launch.py │ ├── launch_tests/ │ │ ├── test_cia402_driver.py │ │ ├── test_proxy_driver.py │ │ ├── test_proxy_driver_namespaced.py │ │ ├── test_proxy_lifecycle_driver.py │ │ └── test_robot_control.py │ ├── package.xml │ ├── rviz/ │ │ └── robot_controller.rviz │ └── urdf/ │ ├── canopen_system/ │ │ ├── canopen_system.ros2_control.xacro │ │ └── canopen_system.urdf.xacro │ ├── cia402_system/ │ │ ├── cia402_system.ros2_control.xacro │ │ └── cia402_system.urdf.xacro │ └── robot_controller/ │ ├── robot_controller.macro.xacro │ ├── robot_controller.ros2_control.xacro │ └── robot_controller.urdf.xacro ├── canopen_utils/ │ ├── CHANGELOG.rst │ ├── LICENSE │ ├── canopen_utils/ │ │ ├── __init__.py │ │ ├── cyclic_tester.py │ │ ├── launch_test_node.py │ │ ├── simple_rpdo_tpdo_tester.py │ │ └── test_node.py │ ├── no_tests/ │ │ ├── _test_copyright.py │ │ ├── _test_flake8.py │ │ └── _test_pep257.py │ ├── package.xml │ ├── resource/ │ │ └── canopen_utils │ ├── setup.cfg │ └── setup.py └── lely_core_libraries/ ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── cmake/ │ └── lely_core_libraries-extras.cmake ├── cogen/ │ ├── __init__.py │ └── cogen.py ├── package.xml ├── patches/ │ └── 0001-Fix-dcf-tools.patch └── setup.cfg ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ --- Language: Cpp BasedOnStyle: Google AccessModifierOffset: -2 AlignAfterOpenBracket: AlwaysBreak BreakBeforeBraces: Allman ColumnLimit: 100 ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 IndentWidth: 2 TabWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: true IncludeBlocks: Preserve ... ================================================ FILE: .codespellignore ================================================ ons ================================================ FILE: .github/ISSUE_TEMPLATE/bug_report.md ================================================ --- name: Bug report about: Create a report to help us improve title: '' labels: bug assignees: '' --- **Describe the bug** A clear and concise description of what the bug is. **To Reproduce** Steps to reproduce the behavior **Expected behavior** A clear and concise description of what you expected to happen. **Logs** **Setup:** - Device: - OS: - ROS-Distro: - Branch/Commit: **Additional context** Add any other context about the problem here. ================================================ FILE: .github/ISSUE_TEMPLATE/feature_request.md ================================================ --- name: Feature request about: Suggest an idea for this project title: '' labels: enhancement assignees: '' --- **Is your feature request related to a problem? Please describe.** A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] **Describe the solution you'd like** A clear and concise description of what you want to happen. **Describe alternatives you've considered** A clear and concise description of any alternative solutions or features you've considered. **Additional context** Add any other context or screenshots about the feature request here. ================================================ FILE: .github/ISSUE_TEMPLATE/question.md ================================================ --- name: Question about: Create a report to help us improve title: '' labels: question assignees: '' --- **Describe the bug** A clear and concise description of what the bug is. **Logs** Add build or execution log for context. **Setup:** - Device: - OS: - ROS-Distro: - Branch/Commit: **Additional context** Add any other context about the problem here. ================================================ FILE: .github/dependabot.yaml ================================================ # Set update schedule for GitHub Actions version: 2 updates: - package-ecosystem: "github-actions" directory: "/" schedule: # Check for updates to GitHub Actions every week interval: "weekly" ================================================ FILE: .github/workflows/ci-format.yml ================================================ # This is a format job. Pre-commit has a first-party GitHub action, so we use # that: https://github.com/pre-commit/action name: Pre-commit Format on: workflow_dispatch: pull_request: jobs: pre-commit: name: Format runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v6 - uses: actions/setup-python@v6 with: python-version: 3.10.6 - name: Install system hooks run: sudo apt update && sudo apt install -qq clang-format-14 cppcheck - uses: pre-commit/action@v3.0.1 with: extra_args: --all-files --hook-stage manual ================================================ FILE: .github/workflows/ci-ros-lint.yml.backup ================================================ name: ROS Lint on: pull_request: jobs: ament_lint: name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: fail-fast: false matrix: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling linter: ${{ matrix.linter }} package-name: $PKG_NAME$ ament_lint_100: name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: fail-fast: false matrix: linter: [cpplint] steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" package-name: ros2_canopen ================================================ FILE: .github/workflows/docker.yml ================================================ name: Create and publish a Docker image on: push: branches: ['master'] workflow_dispatch: env: REGISTRY: ghcr.io IMAGE_NAME: ${{ github.repository }} jobs: build-and-push-image: runs-on: ubuntu-22.04 permissions: contents: read packages: write steps: - name: Checkout repository uses: actions/checkout@v6 - name: Log in to the Container registry uses: docker/login-action@v4 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} password: ${{ secrets.GITHUB_TOKEN }} - name: Extract metadata (tags, labels) for Docker id: meta uses: docker/metadata-action@v6 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} - name: Build and push Docker image uses: docker/build-push-action@v7 with: context: . push: true tags: ${{ steps.meta.outputs.tags }} labels: ${{ steps.meta.outputs.labels }} ================================================ FILE: .github/workflows/humble.yml ================================================ name: Industrial CI Humble on: push: branches: [ humble ] pull_request: branches: [ humble ] workflow_dispatch: jobs: industrial_ci: name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: ROS_DISTRO: [humble] ROS_REPO: [testing] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" steps: - uses: actions/checkout@v6 - uses: actions/cache@v4 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} restore-keys: | ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- - uses: 'ros-industrial/industrial_ci@master' env: BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} ================================================ FILE: .github/workflows/humble_documentation.yml ================================================ name: Documentation Humble # Controls when the workflow will run on: # Triggers the workflow on push or pull request events but only for the master branch push: branches: [ humble ] paths: - 'canopen/**' - '.github/workflows/humble_documentation.yml' # Allows you to run this workflow manually from the Actions tab workflow_dispatch: # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: # This workflow contains a single job called "build" build_documentation: # The type of runner that the job will run on runs-on: ubuntu-22.04 # Steps represent a sequence of tasks that will be executed as part of the job steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v6 - name: Install dependencies run: | sudo apt-get update -qq sudo apt-get install -y -qq doxygen graphviz plantuml pip install sphinx-rtd-theme pip install sphinxcontrib-plantuml pip install sphinx-mdinclude pip install breathe pip install exhale - name: Build doxygen run: | cd ./canopen/doxygen/ doxygen cd ../.. - name: Build documentation run: | cd ./canopen/sphinx/ make html cd ../.. - name: Create commit run: | git clone https://github.com/ros-industrial/ros2_canopen.git --branch gh-pages --single-branch gh-pages mkdir -p gh-pages/manual/humble/ mkdir -p gh-pages/api/humble/ cp -r ./canopen/sphinx/_build/html/* gh-pages/manual/humble/ cp -r ./canopen/doxygen/_build/html/* gh-pages/api/humble/ cd gh-pages git config --local user.email "action@github.com" git config --local user.name "GitHub Action" git add . git commit -m "Update documentation" -a || true - name: Push changes uses: ad-m/github-push-action@master with: branch: gh-pages directory: gh-pages github_token: ${{ secrets.GITHUB_TOKEN }} ================================================ FILE: .github/workflows/prerelease-rolling.yml ================================================ name: Prerelease-Test Rolling on: workflow_dispatch: push: tags: - "*" jobs: industrial_ci: name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: ROS_DISTRO: [rolling] ROS_REPO: [testing] steps: - uses: actions/checkout@v6 - uses: 'ros-industrial/industrial_ci@master' env: BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} PRERELEASE: true ================================================ FILE: .github/workflows/rolling.yml ================================================ name: Industrial CI on: push: branches: [ master ] pull_request: branches: [ master ] workflow_dispatch: jobs: industrial_ci: name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) runs-on: ubuntu-24.04 strategy: fail-fast: false matrix: include: - ROS_DISTRO: rolling ROS_REPO: testing - ROS_DISTRO: jazzy ROS_REPO: testing - ROS_DISTRO: kilted ROS_REPO: testing env: CCACHE_DIR: "${{ github.workspace }}/.ccache" steps: - uses: actions/checkout@v6 - uses: actions/cache@v4 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} restore-keys: | ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- - uses: 'ros-industrial/industrial_ci@master' if: ${{ github.event_name != 'pull_request' || matrix.ROS_DISTRO == 'rolling' }} env: BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} PRERELEASE: false ================================================ FILE: .github/workflows/rolling_documentation.yml ================================================ name: Documentation # Controls when the workflow will run on: # Triggers the workflow on push or pull request events but only for the master branch push: branches: [ master ] paths: - 'canopen/**' - '.github/workflows/rolling_documentation.yml' # Allows you to run this workflow manually from the Actions tab workflow_dispatch: # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: # This workflow contains a single job called "build" build_documentation: # The type of runner that the job will run on runs-on: ubuntu-24.04 strategy: fail-fast: false matrix: ROS_DISTRO: [rolling, jazzy, kilted] # Steps represent a sequence of tasks that will be executed as part of the job steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v6 - name: Install dependencies run: | sudo apt-get update -qq sudo apt-get install -y -qq doxygen graphviz plantuml pip install sphinx-rtd-theme pip install sphinxcontrib-plantuml pip install sphinx-mdinclude pip install breathe pip install exhale - name: Build doxygen run: | cd ./canopen/doxygen/ doxygen cd ../.. - name: Build documentation run: | cd ./canopen/sphinx/ make html cd ../.. - name: Create commit run: | git clone https://github.com/ros-industrial/ros2_canopen.git --branch gh-pages --single-branch gh-pages mkdir -p gh-pages/manual/${{ matrix.ROS_DISTRO }}/ mkdir -p gh-pages/api/${{ matrix.ROS_DISTRO }}/ cp -r ./canopen/sphinx/_build/html/* gh-pages/manual/${{ matrix.ROS_DISTRO }}/ cp -r ./canopen/doxygen/_build/html/* gh-pages/api/${{ matrix.ROS_DISTRO }}/ cd gh-pages git config --local user.email "action@github.com" git config --local user.name "GitHub Action" git add . git commit -m "Update documentation" -a || true - name: Push changes uses: ad-m/github-push-action@master with: branch: gh-pages directory: gh-pages github_token: ${{ secrets.GITHUB_TOKEN }} ================================================ FILE: .gitignore ================================================ .vscode/ **/doc_output/ **/__pycache__/ **/_build **/api ================================================ FILE: .pre-commit-config.yaml ================================================ # To use: # # pre-commit run -a # # Or: # # pre-commit install # (runs every time you commit in git) # # To update this file: # # pre-commit autoupdate # # See https://github.com/pre-commit/pre-commit exclude: '.*\.patch' repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.6.0 hooks: - id: check-added-large-files - id: check-ast - id: check-case-conflict - id: check-docstring-first - id: check-merge-conflict - id: check-symlinks - id: check-xml - id: check-yaml exclude: canopen_core/test/bus_configs/bad_driver_duplicate.yml - id: debug-statements - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace - id: fix-byte-order-marker # Python hooks - repo: https://github.com/asottile/pyupgrade rev: v3.17.0 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] # CPP hooks - repo: local hooks: - id: clang-format name: clang-format description: Format files with ClangFormat. entry: clang-format-14 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] # Cmake hooks # - repo: local # hooks: # - id: ament_lint_cmake # name: ament_lint_cmake # description: Check format of CMakeLists.txt files. # stages: [commit] # entry: ament_lint_cmake # language: system # files: CMakeLists\.txt$ # exclude: lely_core_libraries\/CMakeLists\.txt$ # # Copyright - repo: local hooks: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. stages: [commit] entry: ament_copyright args: ["--add-missing", "ROS-Industrial", "apache2"] language: system # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ - id: rst-directive-colons - id: rst-inline-touching-normal # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell rev: v2.2.1 hooks: - id: codespell args: ['--write-changes', '-I', '.codespellignore'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio|dcf|eds)$ ================================================ FILE: Dockerfile ================================================ FROM ros:rolling-ros-core RUN apt-get update \ && apt-get install -y \ python3-rosdep \ python3-argcomplete \ python3-colcon-common-extensions \ build-essential \ pkg-config \ python3-wheel WORKDIR /home/can_ws/src COPY . ros2_canopen WORKDIR /home/can_ws/ RUN . /opt/ros/rolling/setup.sh \ && rosdep init && rosdep update \ && rosdep install --from-paths src --ignore-src -r -y \ && colcon build \ && . install/setup.sh ================================================ FILE: README.md ================================================ # ROS2 CANopen ## Status | Build Process | Status | |---------------|--------| | Industrial CI Build | [![Industrial CI](https://github.com/ros-industrial/ros2_canopen/actions/workflows/rolling.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/rolling.yml) | | Documentation Build | [![Documentation](https://github.com/ros-industrial/ros2_canopen/actions/workflows/rolling_documentation.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/rolling_documentation.yml) | | Buildfarm Build (rolling) | [![Buildfarm Status](https://build.ros2.org/job/Rdev__ros2_canopen__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__ros2_canopen__ubuntu_noble_amd64/) | | Buildfarm Build (kilted) | [![Buildfarm Status](https://build.ros2.org/job/Kdev__ros2_canopen__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Kdev__ros2_canopen__ubuntu_noble_amd64/) | | Buildfarm Build (jazzy) | [![Buildfarm Status](https://build.ros2.org/job/Jdev__ros2_canopen__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Jdev__ros2_canopen__ubuntu_noble_amd64/) | The stack is currently under development and not yet ready for production use. ## Documentation The documentation consists of two parts: a manual and an api reference. The documentation is built for rolling (master), kilted, jazzy, iron and humble and hosted on github pages. Older ROS 2 releases are EOL and are not supported anymore. ***Note:** Master branch works with ROS2 **Jazzy**, **Kilted** and **Rolling** distributions. For **Humble** distribution use the `humble` branch.* ### Rolling * Manual: https://ros-industrial.github.io/ros2_canopen/manual/rolling/ * API reference: https://ros-industrial.github.io/ros2_canopen/api/rolling/ ### Humble * Manual: https://ros-industrial.github.io/ros2_canopen/manual/humble/ * API reference: https://ros-industrial.github.io/ros2_canopen/api/humble/ ## Features These are some of the features this stack implements. For further information please refer to the documentation. * **YAML-Bus configuration** This canopen stack enables you to configure the bus using a YAML file. In this file you define the nodes that are connected to the bus by specifying their node id, the corresponding EDS file and the driver to run for the node. You can also specify further parameters that overwrite EDS parameters or are inputs to the driver. * **Service based operation** The stack can be operated using standard ROS2 nodes. In this case the device container will load the drivers for master and slave nodes. Each driver will be visible as a node and expose a ROS 2 interface. All drivers are brought up when the device manager is launched. * **Managed service based operation** The stack can be opeprated using managed ROS2 nodes. In this case the device container will load the drivers for master and slave nodes based on the bus configuration. Each driver will be a lifecycle node and expose a ROS 2 interface. The lifecycle manager can be used to bring all device up and down in the correct sequence. * **ROS2 control based operation** Currently, multiple ros2_control interfaces are available. These can be used for controlling CANopen devices. The interfaces are: * canopen_ros2_control/CANopenSystem * canopen_ros2_control/CIA402System * canopen_ros2_control/RobotSystem * **CANopen drivers** Currently, the following drivers are available: * ProxyDriver * Cia402Driver ## Post testing To test stack after it was built from source you should first setup a virtual can network. ```bash sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set vcan0 txqueuelen 1000 sudo ip link set up vcan0 ``` Then you can launch a managed example ```bash ros2 launch canopen_tests cia402_lifecycle_setup.launch.py ros2 lifecycle set /lifecycle_manager configure ros2 lifecycle set /lifecycle_manager activate ``` Or you can launch a standard example ```bash ros2 launch canopen_tests cia402_setup.launch.py ``` Or you can launch a ros2_control example ```bash ros2 launch canopen_tests robot_control_setup.launch.py ``` ## Contributing This repository uses `pre-commit` for code formatting. This program has to be setup locally and installed inside the repository. For this execute in the repository folder following commands: ``` sudo apt install -y pre-commit pre-commit install ``` The checks are automatically executed before each commit. This helps you to always commit well formatted code. To run all the checks manually use `pre-commit run -a` command. For the other options check `pre-commit --help`. In a case of an "emergency" you can avoid execution of pre-commit hooks by adding `-n` flag to `git commit` command - this is NOT recommended to do if you don't know what are you doing! ## Rolling Distribution (Noble & RHEL9) | Package | Noble (Ubuntu) | RHEL9 | |--------------------------|---------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------| | canopen_interfaces | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/) | | lely_core_libraries | [![Build Status](https://build.ros2.org/job/Rbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/) | | canopen_core | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_core__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/) | | canopen_master_driver | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/) | | canopen_base_driver | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/) | | canopen_proxy_driver | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/) | | canopen_402_driver | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/) | | canopen_ros2_control | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/) | | canopen_ros2_controllers | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/) | | canopen_tests | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/) | | canopen_utils | [![Build Status](https://build.ros2.org/job/Rbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/) | ## Kilted Distribution (Noble & RHEL9) | Package | Noble (Ubuntu) | RHEL9 | |--------------------------|---------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------| | canopen_interfaces | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/) | | lely_core_libraries | [![Build Status](https://build.ros2.org/job/Kbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/) | | canopen_core | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_core__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/) | | canopen_master_driver | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/) | | canopen_base_driver | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/) | | canopen_proxy_driver | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/) | | canopen_402_driver | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/) | | canopen_ros2_control | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/) | | canopen_ros2_controllers | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/) | | canopen_tests | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/) | | canopen_utils | [![Build Status](https://build.ros2.org/job/Kbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Kbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Kbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/) | ## Jazzy Distribution (Noble & RHEL9) | Package | Noble (Ubuntu) | RHEL9 | |--------------------------|---------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------| | canopen_interfaces | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_interfaces__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/) | | lely_core_libraries | [![Build Status](https://build.ros2.org/job/Jbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__lely_core_libraries__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/) | | canopen_core | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_core__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/) | | canopen_master_driver | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_master_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/) | | canopen_base_driver | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_base_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/) | | canopen_proxy_driver | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_proxy_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/) | | canopen_402_driver | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_402_driver__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/) | | canopen_ros2_control | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_ros2_control__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/) | | canopen_ros2_controllers | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_ros2_controllers__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/) | | canopen_tests | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_tests__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/) | | canopen_utils | [![Build Status](https://build.ros2.org/job/Jbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__canopen_utils__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Jbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/) | ================================================ FILE: canopen/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * add pdo 6077 torque actual value to the joint state interface as effort (`#316 `_) * Fix configuration parsing and logging * Refactor on_init method for improved readability and consistency * Fix deprecated hardware_interface API (`#386 `_) * Fix typos in warning messages and comments for clarity * `#379 `_: Fix data conversion in the Lely Bridge to enable more data types and proper handling of Emcy in ros2_control * Return error on Emcy and add correct data conversion for Emcy * Fixed types handling in canopen_ros2_control and optimized debug output * Fixed sending values * `#376 `_: increase boot timeout * `#400 `_: Update controllers for ROS Rolling API changes and fix fake slave type handling * Refactor GetValue method to use std::type_index for type comparisons * `motion_generator` as shared library (`#295 `_) * Enhance SimpleSlave class with vendor ID parsing and adjust bus configuration timeouts * Multiple SDO types for slaves (`#278 `_) * Enhance command interface error handling and feedback reporting in CanopenProxyController * Contributors: Christoph Fröhlich, Christoph Hellmann Santos, Dr. Denis Stogl, Patrick Roncagliolo, Vishnuprasad Prachandabhanu, synsi23b 0.3.1 (2025-06-23) ------------------ * Sync upstream 'master' into homing_timeout_pr. Fix homing info message to include both offset and homing timeout information. * Add namespacing support * Contributors: Vishnuprasad Prachandabhanu 0.3.0 (2024-12-12) ------------------ * Update CiA402 bus config docs * Remove set heartbeat service from master documentation (`#294 `_) Co-authored-by: Christoph Hellmann Santos * Add cyclic torque mode to cia402 driver and robot system controller (`#293 `_) * Add base functions for switching to cyclic torque mode * Add cyclic torque mode as effort interface to robot_system controller * Add documentation about cyclic torque mode. Co-authored-by: Christoph Hellmann Santos 0.2.12 (2024-04-22) ------------------- * Merge pull request `#280 `_ from ipa-vsp/fix/yaml-build-error Fix undefined reference to yaml library * pre-commit update * Merge pull request `#261 `_ from gsalinas/configurable-sdo-timeout Configurable SDO timeout * 0.2.9 * forthcoming * Merge pull request `#272 `_ from ros-industrial/ipa-vsp-patch-1 Update maintainer list * Update package.xml * Merge pull request `#220 `_ from ipa-vsp/feature/timeout-config Add timeouts * Add SDO timeout to device config documentation. * change from 100ms to 2000ms * timeout for booting slave * Contributors: Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ * Documentation: Fix launch file spelling (`#208 `_) * Documentation: Fix package creation command. (`#176 `_) * Fix package creation * Fixed bus.yml nodes list * docs: fixed launch file path in instructions Co-authored-by: waterlinux * Contributors: Lewe Christiansen, Vishnuprasad Prachandabhanu 0.2.9 (2024-04-16) ------------------ * Update maintainer list * Update package.xml * Add timeouts * Contributors: Vishnuprasad Prachandabhanu 0.2.7 (2023-06-30) ------------------ * Update ros2_control docs. * Contributors: Christoph Hellmann Santos, Dr. Denis, Xi Huang 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu ================================================ FILE: canopen/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen NONE) find_package(ament_cmake REQUIRED) ament_package() ================================================ FILE: canopen/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen/doxygen/Doxyfile ================================================ # All settings not listed here will use the Doxygen default values. PROJECT_NAME = "ros2_canopen" PROJECT_NUMBER = master PROJECT_BRIEF = "C++ ROS CANopen Library" # Use these lines to include the generated logging.hpp (update install path if needed) INPUT = ../../canopen_core/include ../../canopen_base_driver/include ../../canopen_402_driver/include ../../canopen_proxy_driver/include RECURSIVE = YES OUTPUT_DIRECTORY = _build EXTRACT_ALL = YES SORT_MEMBER_DOCS = NO GENERATE_LATEX = NO GENERATE_XML = YES ENABLE_PREPROCESSING = YES DOT_GRAPH_MAX_NODES = 101 FILE_PATTERNS = *.cpp *.h *.hpp *.md ================================================ FILE: canopen/package.xml ================================================ canopen 0.3.2 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Vishnuprasad Prachandabhanu Apache-2.0 ament_cmake canopen_interfaces canopen_core canopen_base_driver lely_core_libraries canopen_proxy_driver canopen_base_driver canopen_402_driver ament_cmake ================================================ FILE: canopen/sphinx/Makefile ================================================ # Minimal makefile for Sphinx documentation # # You can set these variables from the command line, and also # from the environment for the first two. SPHINXOPTS ?= SPHINXBUILD ?= sphinx-build SOURCEDIR = . BUILDDIR = _build # Put it first so that "make" without argument is like "make help". help: @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) .PHONY: help Makefile clean clean: rm -rf "_doxygen/" "api/" @$(SPHINXBUILD) -M clean "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) # Catch-all target: route all unknown targets to Sphinx using the new # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). %: Makefile @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) ================================================ FILE: canopen/sphinx/_static/css/custom.css ================================================ .wy-table-responsive table td, .wy-table-responsive table th { white-space:normal; } tr { white-space: normal; } thead { white-space: normal; } table { white-space: normal; } ================================================ FILE: canopen/sphinx/application/prbt_robot.rst ================================================ Pilz manipulator PRBT 6 ======================= This guide provides instructions on how to set up Pilz manipulators PRBT 6 using ros2_canopen. Before following this guide, make sure to refer to the user guide to :doc:`../user-guide/configuration`, :doc:`../user-guide/how-to-create-a-configuration` and :doc:`../user-guide/how-to-create-a-robot-system`. Additionally, this guide is based on: `pilz_robots `_. To configure the PRBT 6 using ros2_canopen, please refer to the pre-existing configuration in the `pilz_robot `_ package. Package creation and setup ---------------------------- - Create a new configuration package for the robot with the name ``prbt_robot_support``. .. code-block:: console $ ros2 pkg create --dependencies ros2_canopen lely_core_libraries --build-type ament_cmake prbt_robot_support $ cd prbt_robot_support $ rm -rf src $ rm -rf include $ mkdir -p launch $ mkdir -p config $ mkdir -p urdf $ touch config/prbt_ros2_control.yaml - Create a new bus configuration folder with name ``prbt``. And create ``bus.yml`` and copy ``prbt_0_1.dcf`` from `pilz_support `_ package. Now the package structure should look like this: :: prbt_robot_support ├── config │ ├── prbt │ | ├── bus.yml │ | └── prbt_0_1.dcf │ └── prbt_ros2_control.yaml ├── launch ├── urdf ├── CMakeLists.txt └── package.xml - Add the following to the ``bus.yml`` .. code-block:: yaml options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "prbt_0_1.dcf" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 heartbeat_producer: 1000 switching_state: 2 position_mode: 7 scale_pos_from_dev: 0.00001745329252 scale_pos_to_dev: 57295.7795131 sdo: - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period. tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation - {index: 0x60C1, sub_index: 1} # interpolated position data 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position nodes: prbt_joint_1: node_id: 3 prbt_joint_2: node_id: 4 prbt_joint_3: node_id: 5 prbt_joint_4: node_id: 6 prbt_joint_5: node_id: 7 prbt_joint_6: node_id: 8 - Create ``prbt.ros2_control.xacro`` file in ``urdf`` folder and add the following to the file: .. code-block:: xml canopen_ros2_control/RobotSystem ${bus_config} ${master_config} ${can_interface_name} "${master_bin}" prbt_joint_1 prbt_joint_2 prbt_joint_3 prbt_joint_4 prbt_joint_5 prbt_joint_6 - Add the following to the ``prbt_ros2_control.yaml`` file: .. code-block:: yaml controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster arm_controller: type: joint_trajectory_controller/JointTrajectoryController arm_controller: ros__parameters: joints: - prbt_joint_1 - prbt_joint_2 - prbt_joint_3 - prbt_joint_4 - prbt_joint_5 - prbt_joint_6 command_interfaces: - position state_interfaces: - position - velocity stop_trajectory_duration: 0.2 state_publish_rate: 100.0 action_monitor_rate: 25.0 goal_time: 0.0 limits: prbt_joint_1: has_acceleration_limits: true max_acceleration: 6.0 prbt_joint_2: has_acceleration_limits: true max_acceleration: 6.0 prbt_joint_3: has_acceleration_limits: true max_acceleration: 6.0 prbt_joint_4: has_acceleration_limits: true max_acceleration: 6.0 prbt_joint_5: has_acceleration_limits: true max_acceleration: 6.0 prbt_joint_6: has_acceleration_limits: true max_acceleration: 6.0 - Create a launch file with name ``robot.launch.py`` in the launch directory of your package. Follow the instructions :doc:`../user-guide/how-to-create-a-robot-system` to complete the launch file. The launch file should look like this: .. code-block:: python import os from ament_index_python import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros def generate_launch_description(): declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", default_value="prbt_robot_support" ) ) declared_arguments.append( DeclareLaunchArgument( "can_interface_name", default_value="vcan0", description="Interface name for can", ) ) declared_arguments.append( DeclareLaunchArgument( "use_ros2_control", default_value="true", description="Use ros2_control", ) ) controller_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt_ros2_control.yaml"]) bus_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt", "bus.yml"]) master_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt", "master.dcf"]) can_interface_name = LaunchConfiguration("can_interface_name") master_bin_path = os.path.join( get_package_share_directory("prbt_robot_support"), "config", "prbt", "master.bin", ) if not os.path.exists(master_bin_path): master_bin_path = "" robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution([FindPackageShare(LaunchConfiguration("description_package")), "urdf", "prbt.xacro"]), " ", "bus_config:=", bus_config, " ", "master_config:=", master_config, " ", "master_bin:=", master_bin_path, " ", "can_interface_name:=", can_interface_name, ] ) robot_description = {"robot_description": launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str)} robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) # Controller manager controller_manager_node = Node( package="controller_manager", executable="ros2_control_node", output="both", parameters=[robot_description, controller_config], ) joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) arm_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["arm_controller", "--controller-manager", "/controller_manager"], ) nodes_list = [ robot_state_publisher_node, controller_manager_node, joint_state_broadcaster_spawner, arm_controller_spawner, ] return LaunchDescription(declared_arguments + nodes_list) - Edit the CMakeLists.txt file of your package and add the following lines after the find_package section. .. code-block:: cmake cogen_dcf(prbt) install(DIRECTORY launch urdf meshes DESTINATION share/${PROJECT_NAME}) install(FILES config/prbt_ros2_control.yaml DESTINATION share/${PROJECT_NAME}/config) - Build your package and source the setup file. MoveIt2 setup ------------- Follow the `MoveIt Setup Assistance `_ tutorial and create the MoveIt2 package for your robot. The MoveIt2 package should be created in the same workspace as your robot package. - Update ``moveit_controller.yaml`` file in the config directory of your MoveIt2 package. The file should look like this: .. code-block:: yaml # MoveIt uses this configuration for controller management trajectory_execution: allowed_execution_duration_scaling: 1.2 allowed_goal_duration_margin: 0.5 allowed_start_tolerance: 0.01 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager moveit_simple_controller_manager: controller_names: - arm_controller arm_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: True joints: - prbt_joint_1 - prbt_joint_2 - prbt_joint_3 - prbt_joint_4 - prbt_joint_5 - prbt_joint_6 - Create a launch file ``moveit_planning_execution.launch.py`` to launch moveit and robot components. .. code-block:: python from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import TimerAction from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): moveit_config = MoveItConfigsBuilder( "prbt", package_name="prbt_robot_moveit_config" ).to_moveit_configs() declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( "can_interface_name", default_value="can0", description="Interface name for can", ) ) robot_hw_node = IncludeLaunchDescription( PythonLaunchDescriptionSource( [PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "launch", "robot.launch.py"])], ), launch_arguments={ "can_interface_name": LaunchConfiguration("can_interface_name"), }.items(), ) virtual_joints = IncludeLaunchDescription( PythonLaunchDescriptionSource( str( moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" ) ), ) move_group = IncludeLaunchDescription( PythonLaunchDescriptionSource( str(moveit_config.package_path / "launch/move_group.launch.py") ), ) rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource( str(moveit_config.package_path / "launch/moveit_rviz.launch.py") ), ) node_list = [ robot_hw_node, virtual_joints, move_group, rviz, ] return LaunchDescription(declared_arguments + node_list) Running the PRBT robot ------------------------ There are two ways to run the demo. First you can use ``canopen_fake_slave`` to simulate the robot using CAN interface ``vcan0``. Second you can use the real robot. Refer :doc:`../quickstart/operation` to configure CAN interface. .. code-block:: bash # Run the demo using fake robot ros2 launch prbt_robot_moveit_config moveit_planning_execution.launch.py can_interface_name:=vcan0 # Run the demo using real robot ros2 launch prbt_robot_moveit_config moveit_planning_execution.launch.py can_interface_name:=can0 Rviz2: .. image:: ../images/prbt_rviz2.png :width: 90% :align: center ================================================ FILE: canopen/sphinx/application/trinamic.rst ================================================ Trinamic Stepper Motor control ============================== Introduction ------------ This tutorial is a simple example of how to use the ros2_canopen package to control a `Trinamic smart stepper motor `_. Getting started --------------- If you haven't already done so, follow the steps in the :doc:`../user-guide/configuration`. Configuration ------------- - Create a new folder in the ``config`` folder of your configuration package. Name it ``single-pd42``. - Download ``.eds`` file from `Trinamic `_ and place ``TMCM-1270.eds`` in the ``single-pd42`` folder. - Create a ``bus.yml`` file in the ``single-pd42`` folder with the following content: .. code-block:: yaml options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 2 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 20000 defaults: dcf: "TMCM-1270.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" polling: false heartbeat_producer: 1000 # Heartbeat every 1000 ms sdo: # SDO executed during config - {index: 0x6081, sub_index: 0, value: 500000} # Set velocity - {index: 0x6083, sub_index: 0, value: 1000000} # Set acceleration - {index: 0x6083, sub_index: 0, value: 1000000} # Set deceleration - {index: 0x6085, sub_index: 0, value: 1000000} # Set quickstop deceleration - {index: 0x6098, sub_index: 0, value: 35} # Set default homing mode to 35 - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s nodes: trinamic_pd42: node_id: 1 - Edit the ``CMakeLists.txt`` file in the ``config`` folder of your configuration package and add the following lines: .. code-block:: cmake cmake_minimum_required(VERSION 3.8) project(trinamic_pd42_can) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_srvs REQUIRED) find_package(canopen REQUIRED) find_package(lely_core_libraries REQUIRED) find_package(canopen_interfaces REQUIRED) # generate_dcf(single-pd42) cogen_dcf(single-pd42) add_executable(position_tick_client src/position_tick_motor.cpp) target_link_libraries(position_tick_client ${canopen_interfaces_TARGETS} ${std_srvs_TARGETS} rclcpp::rclcpp ) install(TARGETS position_tick_client DESTINATION lib/${PROJECT_NAME}) # install launch file install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) endif() ament_package() - Create launch file in folder ``launch`` and add the following content: .. code-block:: python import os import sys import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python import get_package_share_directory from launch import LaunchDescription def generate_launch_description(): ld = LaunchDescription() slave_eds_path = os.path.join( get_package_share_directory("trinamic_pd42_can"), "config", "single-pd42", "TMCM-1270.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "1", "node_name": "pd42_slave", "slave_config": slave_eds_path, }.items(), ) master_bin_path = os.path.join( get_package_share_directory("trinamic_pd42_can"), "config", "single-pd42", "master.bin", ) if not os.path.exists(master_bin_path): master_bin_path = "" device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("trinamic_pd42_can"), "config", "single-pd42", "master.dcf", ), "master_bin": master_bin_path, "bus_config": os.path.join( get_package_share_directory("trinamic_pd42_can"), "config", "single-pd42", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) ld.add_action(device_container) ld.add_action(slave_node_1) return ld Running the example ------------------- To begin, follow the instructions for :doc:`../quickstart/operation`, which can be done using either a virtual or peak CAN interface. If you prefer to use a real CAN interface, you will need to modify the launch file by changing the ``can_interface_name`` argument to ``can0``. Additionally, if you are using real hardware, you should comment out the fake slave launch by adding a *#* in front of the line *ld.add_action(slave_node_1)*. Once these changes have been made, you can launch the example. .. code-block:: console ros2 launch trinamic_pd42_can .launch.py Initilaize the motor by calling the service ``/trinamic_pd42/init``: .. code-block:: console ros2 service call /trinamic_pd42/init std_srvs/srv/Trigger Set the operation mode to ``Profile Position Mode`` by calling the service ``/trinamic_pd42/position_mode``: .. code-block:: console ros2 service call /trinamic_pd42/position_mode std_srvs/srv/Trigger Set the target to the motor by calling the service ``/trinamic_pd42/target``: .. code-block:: console ros2 service call /trinamic_pd42/target canopen_interfaces/srv/COTargetDouble "{ target: 10.0 }" Reference --------- You can find the source code for this example in the `trinamic_pd42_can `_ package. ================================================ FILE: canopen/sphinx/conf.py ================================================ # Copyright 2023 ROS-Industrial # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # Configuration file for the Sphinx documentation builder. # # This file only contains a selection of the most common options. For a full # list see the documentation: # https://www.sphinx-doc.org/en/master/usage/configuration.html # -- Path setup -------------------------------------------------------------- # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # from pathlib import Path # -- Project information ----------------------------------------------------- project = "ros2_canopen" copyright = "2022, Christoph Hellmann Santos, Harshavardhan Deshpande" author = "Christoph Hellmann Santos, Harshavardhan Deshpande" # The full version, including alpha/beta/rc tags release = "0.0.1" # -- General configuration --------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ "sphinx_rtd_theme", "sphinx_mdinclude", "sphinx.ext.imgmath", "sphinx.ext.todo", "sphinx.ext.graphviz", "sphinxcontrib.plantuml", "breathe", ] breathe_default_project = "ros2_canopen" def get_package(package: str): path = Path(__file__).parent.parent.parent.joinpath(f"{package}/include/{package}") files_gen = path.glob("*.hpp") files = [] for file in files_gen: files.append(file.name) return (path, files) breathe_projects = { "ros2_canopen": "../doxygen/_build/xml/", } # Tell sphinx what the primary language being documented is. primary_domain = "cpp" # Tell sphinx what the pygments highlight language should be. highlight_language = "cpp" # Add any paths that contain templates here, relative to this directory. templates_path = ["_templates"] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # -- Options for HTML output ------------------------------------------------- # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # html_theme = "sphinx_rtd_theme" # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ["_static"] html_logo = "images/ros2-canopen-logo.png" html_css_files = ["css/custom.css"] pygments_style = "sphinx" ================================================ FILE: canopen/sphinx/developers-guide/architecture.rst ================================================ Architecture ============= The architecture of the ROS2 CANopen stack is based on the composition concept of ROS2. In ROS2 components are dynamically loadable ROS2 nodes which are loaded by a component manager. Device Container """""""""""""""""" The core of the ROS2 CANopen stack is the ros2_canopen::DeviceContainer which implements the rclcpp::ComponentManager class. The DeviceContainer enables loading drivers for the CANopen master and the devices on the bus, that have been exported as rclcpp_components. CANopen Master drivers need to implement the ros2_canopen::CanopenMasterInterface, CANopen Device drivers need to implement the ros2_canopen::CanopenDriverInterface. The difference between the ros2_canopen::DeviceContainer and the rclcpp::ComponentContainer is that the device container loads only master and driver components specified in the bus configuration (bus.yml). It does not have services for loading components online. All components that are connected or will be connected to the CANopen Bus need to be known and specified in the bus.yml before starting the device container. .. uml:: rclcpp::ComponentManager <|-- ros2_canopen::DeviceContainer class rclcpp::ComponentManager { std::weak_ptr executor_ std::vector get_component_resources(std::string package_name) std::shared_ptr create_component_factory(ComponentResource resource) void on_list_nodes(...) virtual void set_executor(const std::weak_ptr executor) } class ros2_canopen::DeviceContainer { std::shared_ptr can_master_ std::shared_ptr lifecycle_manager_ std::map> drivers_ void on_list_nodes(...) override virtual void set_executor(const std::weak_ptr executor) override } The device container will first load the master specified in the bus configuration. Then it will load the drivers specified in the bus configuration. If the master and drivers specified in the bus configuration are managed nodes it will as well load the ros2_canopen::LifecycleManager. CANopen Master Driver Architecture """"""""""""""""""""""""""""""""""" The architecture for CANopen master drivers looks as depicted in the class diagram. All master drivers consist of three main classes. The first class is the functionality class that contains the drivers functionailities independently of the ROS2 node type. This class needs to implement the ros2_canopen::node_interfaces::NodeCanopenMasterInterface. ros2_canopen::node_interfaces::NodeCanopenMaster is an abstract class that provides some useful functionality and implements the ros2_canopen::node_interfaces::NodeCanopenMasterInterface. Usually, master drivers will inherit from ros2_canopen::node_interfaces::NodeCanopenMaster. The second class is the class that wraps the functionality class in a rclcpp::Node. This class should implement the ros2_canopen::CanopenMasterInterface. The canopen_core package provides a convenience class ros2_canopen::CanopenMaster that should be inherited from and implements the ros2_canopen::CanopenMasterInterface. The third class is the class that wraps the functionality class in a rclcpp_lifecycle::LifecycleNode. This class should implement the ros2_canopen::CanopenMasterInterface. The canopen_core package provides a convenience class ros2_canopen::LifecycleCanopenMaster that should be inherited from and implements the ros2_canopen::CanopenMasterInterface. .. uml:: package "canopen_core" { interface ros2_canopen::CanopenMasterInterface interface ros2_canopen::node_interfaces::NodeCanopenMasterInterface abstract ros2_canopen::LifecycleCanopenMaster abstract ros2_canopen::node_interfaces::NodeCanopenMaster abstract ros2_canopen::CanopenMaster ros2_canopen::node_interfaces::NodeCanopenMasterInterface - ros2_canopen::CanopenMaster : < has ros2_canopen::LifecycleCanopenMaster - ros2_canopen::node_interfaces::NodeCanopenMasterInterface : > has ros2_canopen::CanopenMasterInterface <|-- ros2_canopen::LifecycleCanopenMaster ros2_canopen::node_interfaces::NodeCanopenMasterInterface <|-- ros2_canopen::node_interfaces::NodeCanopenMaster ros2_canopen::CanopenMasterInterface <|-- ros2_canopen::CanopenMaster } package "canopen_master_driver" { class ros2_canopen::LifecycleMasterDriver << (C, blue) Component>> class ros2_canopen::node_interfaces::NodeCanopenBasicMaster class ros2_canopen::MasterDriver << (C, blue) Component>> ros2_canopen::LifecycleMasterDriver - ros2_canopen::node_interfaces::NodeCanopenBasicMaster: > has ros2_canopen::node_interfaces::NodeCanopenBasicMaster - ros2_canopen::MasterDriver : < has ros2_canopen::LifecycleCanopenMaster <|-- ros2_canopen::LifecycleMasterDriver ros2_canopen::node_interfaces::NodeCanopenMaster <|-- ros2_canopen::node_interfaces::NodeCanopenBasicMaster ros2_canopen::CanopenMaster <|-- ros2_canopen::MasterDriver } CANopen Device Driver Architecture """"""""""""""""""""""""""""""""""" The architecture for CANopen device drivers looks as depicted in the class diagram. All device drivers consist of three main classes. The first class is the functionality class that contains the drivers functionailities independently of the ROS2 node type. This class needs to implement the ros2_canopen::node_interfaces::NodeCanopenDriverInterface. ros2_canopen::node_interfaces::NodeCanopenDriver is an abstract class that provides some useful functionality and implements the ros2_canopen::node_interfaces::NodeCanopenDriverInterface. If you plan to write a driver from scratch based on Lely Core library, your functionality class should inherit from ros2_canopen::node_interfaces::NodeCanopenDriver. If you want to use the existing lely_driver_bridge, your functionality class should inherit from ros2_canopen::NodeCanopenBaseDriver. The second class is the class that wraps the functionality class in a rclcpp::Node. This class should implement the ros2_canopen::CanopenDriverInterface. The canopen_core package provides a convenience class ros2_canopen::CanopenDriver that should be inherited from and implements the ros2_canopen::CanopenDriverInterface. The third class is the class that wraps the functionality class in a rclcpp_lifecycle::LifecycleNode. This class should implement the ros2_canopen::CanopenDriverInterface. The canopen_core package provides a convenience class ros2_canopen::LifecycleCanopenDriver that should be inherited from and implements the ros2_canopen::CanopenDriverInterface. .. uml:: package "canopen_core" { interface ros2_canopen::CanopenDriverInterface interface ros2_canopen::node_interfaces::NodeCanopenDriverInterface abstract ros2_canopen::LifecycleCanopenDriver abstract ros2_canopen::node_interfaces::NodeCanopenDriver abstract ros2_canopen::CanopenDriver ros2_canopen::node_interfaces::NodeCanopenDriverInterface - ros2_canopen::CanopenDriver : < has ros2_canopen::LifecycleCanopenDriver - ros2_canopen::node_interfaces::NodeCanopenDriverInterface : > has ros2_canopen::CanopenDriverInterface <|-- ros2_canopen::LifecycleCanopenDriver ros2_canopen::node_interfaces::NodeCanopenDriverInterface <|-- ros2_canopen::node_interfaces::NodeCanopenDriver ros2_canopen::CanopenDriverInterface <|-- ros2_canopen::CanopenDriver } package "canopen_base_driver" { class ros2_canopen::LifecycleBaseDriver << (C, blue) Component>> class ros2_canopen::node_interfaces::NodeCanopenBaseDriver class ros2_canopen::BaseDriver << (C, blue) Component>> ros2_canopen::LifecycleBaseDriver - ros2_canopen::node_interfaces::NodeCanopenBaseDriver: > has ros2_canopen::node_interfaces::NodeCanopenBaseDriver - ros2_canopen::BaseDriver : < has ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleBaseDriver ros2_canopen::node_interfaces::NodeCanopenDriver <|-- ros2_canopen::node_interfaces::NodeCanopenBaseDriver ros2_canopen::CanopenDriver <|-- ros2_canopen::BaseDriver } package "canopen_proxy_driver" { class ros2_canopen::LifecycleProxyDriver << (C, blue) Component>> class ros2_canopen::node_interfaces::NodeCanopenProxyDriver class ros2_canopen::ProxyDriver << (C, blue) Component>> ros2_canopen::LifecycleProxyDriver - ros2_canopen::node_interfaces::NodeCanopenProxyDriver: > has ros2_canopen::node_interfaces::NodeCanopenProxyDriver - ros2_canopen::ProxyDriver : < has ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleProxyDriver ros2_canopen::node_interfaces::NodeCanopenBaseDriver <|-- ros2_canopen::node_interfaces::NodeCanopenProxyDriver ros2_canopen::CanopenDriver <|-- ros2_canopen::ProxyDriver } package "canopen_402_driver" { class ros2_canopen::LifecycleCia402Driver << (C, blue) Component>> class ros2_canopen::node_interfaces::NodeCanopen402Driver class ros2_canopen::Cia402Driver << (C, blue) Component>> ros2_canopen::LifecycleCia402Driver - ros2_canopen::node_interfaces::NodeCanopen402Driver: > has ros2_canopen::node_interfaces::NodeCanopen402Driver - ros2_canopen::Cia402Driver : < has ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleCia402Driver ros2_canopen::node_interfaces::NodeCanopenProxyDriver <|-- ros2_canopen::node_interfaces::NodeCanopen402Driver ros2_canopen::CanopenDriver <|-- ros2_canopen::Cia402Driver } ================================================ FILE: canopen/sphinx/developers-guide/design-objectives.rst ================================================ Design Objectives ====================== The ros_canopen stack had a number of drawbacks especially when it came to maintainablity and complexity. In order to address these drawbacks, we have decided to redesign the ros2_canopen stack completely with the following development goals. .. csv-table:: Development Objectives :header-rows: 1 :class: longtable :delim: ; :widths: 1 2 Objective; Description Understandability and Extendability; One major drawback of ros_canopen was that actually extending it with new drivers required to understand the complex stack with its different layers. Robust Parallel Requests; When multiple nodes are running on the same bus, it should be possible to make requests to the nodes concurrently from ROS2 and have the canopen master handle the sequencing. Easy Maintenance; Maintenance effort should be reduced as much as possible. Therefore, a clean and clear code structure and documentation is needed and only funcitonalities that are not already available from other high quality open source libraries should be self implemented. Enable controlling drives via ros2_control; A driver for CIA402 and a ros2_control interface need to be developed. Enable controlling drives via ros2 ervice interface; A driver for CIA402 and a service interface need to be developed. Enable proxy functionalities via ros2 interface; For debugging purposes a proxy driver should be developed, which enables sending and receiving CANopen objects via a ros2 interface. Good enough documentation; Write documentation for using and understanding the ros2_canopen stack. ================================================ FILE: canopen/sphinx/developers-guide/new-device-manager.rst ================================================ Creating your device manager ============================ ================================================ FILE: canopen/sphinx/developers-guide/new-driver.rst ================================================ Creating a new device driver ============================ Creating your own device driver is fairly easy in ros2_canopen. You should do this if you need to create a driver for a specific device or a specific device profile that is not yet supported. If you create a driver for a device profile we are happy to integrate the package into this repository - simply create a PR. What you need to do """""""""""""""""""" To create a new driver you need to implement at least two classes. One being the functional class, that contains your drivers functionalities. The other being a ROS2 wrapper node. Generally we recommend creating one ROS2 wrapper node and another ROS2 lifecycle wrapper node. How you do it """""""""""""" First you need to decide from which extension point you want to start. Usually, this is either the core interface, the base driver or the proxy driver. Base driver provides you with all necessary callbacks for CANopen functionalities but does not come with any ROS2 interface. Proxy driver has a simple forwarding ROS2 interface that is useful for any driver. The core interface comes without any functionality, you need to implement everything on your own. We recommend creating you driver based on Proxy driver, this will be explained here. Create the package ------------------ Create your new package using the standard ros2 pkg commands. Make sure you add the following dependencies: * rclcpp * rclcpp_components * rclcpp_lifecycle * lifecycle_msgs * canopen_core * canopen_interfaces * canopen_base_driver * canopen_proxy_driver * lely_core_libraries * std_msgs * std_srvs Once done add a subfolder ``node_interfaces`` in the ``src/`` and the ``include/[pacakge_name]/`` folders. Create the functionality class ------------------------------ The functionality class is structured similar to a LifecycleNode. The functionality class has the following callback functions that are related to lifecycle which you can override: * void configure(bool called_from_base) * void activate(bool called_from_base) * void deactivate(bool called_from_base) * void cleanup(bool called_from_base) * void shutdown(bool called_from_base) CANopen functionality ********************* In addition to the functions there are callbacks for CANopen functionality that you can override: * void on_rpdo(COData data) * void on_nmt(NmtState nmt_state) To interact with the CANopen device you can use the ros2_canopen::LelyDriverBridge object, that is stored in the functionality class (this->driver_). The ros2_canopen::LelyDriverBridge provides the following functions you should use in your driver: * void nmt_command(NmtState nmt_state) * void tpdo_transmit(COData data) * std::futureasync_sdo_write(COData data) * std::futureasync_sdo_read(COData data) .. note:: The CANopen related functionality can only be used in the activate function or timers/threads that were started by the activate function. If you start timers or threads in the activate function, that use CANopen functionality, these have to be stopped in the deactivate function. ROS2 functionality ****************** ROS2 functionality is available via the ``node_`` object of the functionality class. This object has a templated type and can either be a ``rclcpp::Node`` or ``rclcpp_lifecycle::LifecycleNode``. You can use the standard functions like create_timer, create_publisher etc. .. note:: Currently it seems, that when you use template functions i.e. ``node_->create_publisher(...)`` you need to create a template specialisation. Your class declaration should look like this: .. code-block:: C++ :name: "node_interfaces/node_canopen_xxx_driver.hpp" node_interfaces/node_canopen_xxx_driver.hpp: #include template class NodeCanopenXXXDriver : public NodeCanopenProxyDriver { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); public: NodeCanopenXXXDriver(NODETYPE *node); /*Your overrides etc. go here*/ }; Your member definitions go here: .. code-block:: C++ :name: "node_interfaces/node_canopen_xxx_driver_impl.hpp" node_interfaces/node_canopen_xxx_driver.hpp: #include node_interfaces/node_canopen_xxx_driver.hpp /*Your function definitions go here.*/ Your explicit template instantiations go here: .. code-block:: C++ :name: "node_interfaces/node_canopen_xxx_driver.cpp" node_interfaces/node_canopen_xxx_driver.cpp: #include node_interfaces/node_canopen_xxx_driver.hpp #include node_interfaces/node_canopen_xxx_driver_impl.hpp template class ::node_interfaces::NodeCanopenXXXDriver; template class ::node_interfaces::NodeCanopenXXXDriver; Create the ROS2 wrapper classes ------------------------------- The ROS2 wrapper classes are fairly easy to create once you wrote the functionality class. The wrappers simply use the functionality class to provide the functionality. The ROS2 wrapper class should always be derived from ``ros2_canopen::CanopenDriver`` or ``ros2_canopen::LifecycleCanopenDriver`` . The declaration should look like this: .. code:: lifecycle_xxx_driver.hpp: #include "canopen_xxx_driver/node_interfaces/node_canopen_xxx_driver.hpp" #include "canopen_core/driver_node.hpp" /** * @brief Lifecycle Proxy Driver * * A very basic driver without any functionality. * */ class LifecycleXXXDriver : public ros2_canopen::LifecycleCanopenDriver { std::shared_ptr> node_canopen_xxx_driver_; public: LifecycleXXXDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); }; The definitions should look like this: .. code:: #include "canopen_xxx_driver/lifecycle_proxy_driver.hpp" using namespace ros2_canopen; LifecycleXXXDriver::LifecycleXXXDriver(rclcpp::NodeOptions node_options) : LifecycleCanopenDriver(node_options) { node_canopen_xxx_driver_ = std::make_shared>(this); node_canopen_proxy_driver_ = std::static_pointer_cast(node_canopen_xxx_driver_); node_canopen_driver_ = std::static_pointer_cast(node_canopen_xxx_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleXXXDriver) Adapt the CMakeLists.txt ************************ The CMakeLists.txt file should look like this: .. code:: CMAKE cmake_minimum_required(VERSION 3.8) project(canopen_xxx_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(canopen_base_driver REQUIRED) find_package(canopen_proxy_driver REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) # Functionality library add_library(node_canopen_xxx_driver src/node_interfaces/node_canopen_xxx_driver.cpp ) target_compile_features(node_canopen_xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_xxx_driver PUBLIC -Wl,--no-undefined) target_include_directories(node_canopen_xxx_driver PUBLIC $ $) target_link_libraries(node_canopen_xxx_driver PUBLIC canopen_proxy_driver::node_canopen_proxy_driver ) # Lifecycle driver add_library(lifecycle_xxx_driver src/lifecycle_xxx_driver.cpp ) target_compile_features(lifecycle_xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_xxx_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_xxx_driver PUBLIC $ $) target_link_libraries(lifecycle_xxx_driver PUBLIC canopen_core::node_canopen_driver node_canopen_xxx_driver rclcpp_components::component rclcpp_lifecycle::rclcpp_lifecycle ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(lifecycle_xxx_driver PRIVATE "CANOPEN_XXX_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(lifecycle_xxx_driver "ros2_canopen::LifecycleXXXDriver") set(node_plugins "${node_plugins}ros2_canopen::LifecycleXXXDriver;$\n") # Non lifecycle driver add_library(xxx_driver src/xxx_driver.cpp ) target_compile_features(xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(xxx_driver PUBLIC -Wl,--no-undefined) target_include_directories(xxx_driver PUBLIC $ $) target_link_libraries(xxx_driver PUBLIC canopen_core::node_canopen_driver node_canopen_xxx_driver rclcpp_components::component ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(xxx_driver PRIVATE "CANOPEN_XXX_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(xxx_driver "ros2_canopen::XXXDriver") set(node_plugins "${node_plugins}ros2_canopen::XXXDriver;$\n") install( DIRECTORY include/ DESTINATION include ) install( TARGETS lifecycle_xxx_driver xxx_driver node_canopen_xxx_driver EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) endif() ament_export_include_directories( include ) ament_export_libraries( lifecycle_xxx_driver xxx_driver node_canopen_xxx_driver ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( canopen_base_driver canopen_core canopen_interfaces canopen_proxy_driver rclcpp rclcpp_components rclcpp_lifecycle std_msgs std_srvs ) ament_package() ================================================ FILE: canopen/sphinx/developers-guide/new-master.rst ================================================ Creating a new master ===================== ================================================ FILE: canopen/sphinx/developers-guide/overview.rst ================================================ Overview ======== ros2_canopen provides nodes for interfacing with CANopen devices. The library builds upon the professional and open source lelycore canopen library as opposed to the previous ros_canopen stack. In ros2_canopen the bus configuration is simplified through the use of lelycore's configutaration toolchain. ros2_canopen contains a number of packages that serve different serve different purposes. * **canopen**: Meta-package for easy installation and contains overall documentation of the ros2_canopen stack. * **lely_core_libraries**: A colcon package wrapper for the lelycore canopen library, for convenient installation with rosdep. * **canopen_core**: Contains the core structures of the ros2_canopen stack such as the device manager and the master node and the driver node interface. * **canopen_base_driver**: This package contains the base implementation of a ROS2 CANopen device driver. It can base used by other drivers for easy extension. * **canopen_proxy_driver**: Contains an implementation of a proxy driver which simply forwards CANopen functionality for a specific device via ROS2 services and messages. * **canopen_402_driver**: Contains an implementation of the CIA402 profile for motion controllers and exposes the profiles functionalities via ROS2 services and messages. The implementation is copied from ros_canopen/canopen_402 and this package is licensed accordingly under GNU Lesser General Public License v3.0! ================================================ FILE: canopen/sphinx/index.rst ================================================ ROS2 CANopen Stack ================== This is the documentation of the ROS2 CANopen stack. .. toctree:: :maxdepth: 1 :caption: Quickstart :glob: quickstart/installation quickstart/operation quickstart/examples .. toctree:: :maxdepth: 1 :caption: User Guide :glob: user-guide/operation user-guide/configuration user-guide/master user-guide/proxy-driver user-guide/cia402-driver user-guide/how-to-create-a-configuration user-guide/how-to-create-a-cia301-system user-guide/how-to-create-a-robot-system .. toctree:: :maxdepth: 1 :caption: Developer Guide :glob: developers-guide/design-objectives developers-guide/overview developers-guide/architecture developers-guide/new-driver developers-guide/new-master API Reference .. toctree:: :maxdepth: 1 :caption: Software Tests :glob: software-tests/** .. toctree:: :maxdepth: 1 :caption: Application Demos :glob: application/** ================================================ FILE: canopen/sphinx/make.bat ================================================ @ECHO OFF pushd %~dp0 REM Command file for Sphinx documentation if "%SPHINXBUILD%" == "" ( set SPHINXBUILD=sphinx-build ) set SOURCEDIR=. set BUILDDIR=_build if "%1" == "" goto help %SPHINXBUILD% >NUL 2>NUL if errorlevel 9009 ( echo. echo.The 'sphinx-build' command was not found. Make sure you have Sphinx echo.installed, then set the SPHINXBUILD environment variable to point echo.to the full path of the 'sphinx-build' executable. Alternatively you echo.may add the Sphinx directory to PATH. echo. echo.If you don't have Sphinx installed, grab it from echo.https://www.sphinx-doc.org/ exit /b 1 ) %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% goto end :help %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% :end popd ================================================ FILE: canopen/sphinx/quickstart/examples.rst ================================================ Running Examples ================ In order to tryout the library a few examples are provided in the ``canopen_tests`` directory. You can run them if you have :ref:`started the vcan0 interface `. Service Interface --------------------- .. code-block:: bash ros2 launch canopen_tests cia402_setup.launch.py Managed Service Interface ------------------------- .. code-block:: bash ros2 launch canopen_tests cia402_lifecycle_setup.launch.py ROS2 Control ------------ Proxy Setup ,,,,,,,,,,, .. code-block:: bash ros2 launch canopen_tests canopen_system.launch.py CiA402 Setup ,,,,,,,,,,,, .. code-block:: bash ros2 launch canopen_tests cia402_system.launch.py Robot Setup ,,,,,,,,,,,, .. code-block:: bash ros2 launch canopen_tests robot_control_setup.launch.py ================================================ FILE: canopen/sphinx/quickstart/installation.rst ================================================ Installation =============================== Clone ros2_canopen into your ROS2 workspace's source folder, install dependencies and build with colcon and your done. .. code-block:: console $ git clone https://github.com/ros-industrial/ros2_canopen.git $ cd .. $ rosdep install --from-paths src/ros2_canopen --ignore-src -r -y $ colcon build ================================================ FILE: canopen/sphinx/quickstart/operation.rst ================================================ Setup CAN Controller ==================== .. _quick-start-setup-can-controller: **Option 1**: Virtual CANController .. code-block:: console $ sudo modprobe vcan $ sudo ip link add dev vcan0 type vcan $ sudo ip link set vcan0 txqueuelen 1000 $ sudo ip link set up vcan0 **Option 2**: Peak CANController .. code-block:: console $ sudo modprobe peak_usb $ sudo ip link set can0 up type can bitrate 1000000 $ sudo ip link set can0 txqueuelen 1000 $ sudo ip link set up can0 Bitrate depends on your bus and devices capabilities. **Option 3**: candleLight USB-CAN Adapter .. code-block:: console $ sudo modprobe gs_usb $ sudo ip link set can0 up type can bitrate 500000 $ sudo ip link set can0 txqueuelen 1000 $ sudo ip link set up can0 Bitrate depends on your bus and devices capabilities. **Option 4**: Adapt these steps to other socketcan devices ================================================ FILE: canopen/sphinx/software-tests/proxy-driver-test.rst ================================================ Lifecycle Proxy Driver Test =========================== .. csv-table:: Testdetails :header: "Detail", "Information" :delim: ; Package; canopen_tests Test file; launch_tests/test_lifecycle_proxy_driver.py Description; Tests nmt, sdo and lifecycle of lifecycle proxy driver Prerequisites; vcan0 must be available Proxy Driver Test =========================== .. csv-table:: Testdetails :header: "Detail", "Information" :delim: ; Package; canopen_tests Test file; launch_tests/test_proxy_driver.py Description; Tests nmt, sdo and lifecycle of proxy driver Prerequisites; vcan0 must be available ================================================ FILE: canopen/sphinx/software-tests/ros2_control_system-test.rst ================================================ ros2_control SystemInterface test ================================= Test details ------------ .. csv-table:: Tests :header: "Detail", "Information" :delim: ; Package; canopen_tests Test file; launch/canopen_system.launch.py Description; Create an exemplary ros2_control SystemInterface with CAN master and communicates to a slave node. Prerequisites; vcan0 must be available To bring up vcan0: .. code-block:: bash sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set vcan0 txqueuelen 1000 sudo ip link set up vcan0 Explanation of the test ------------------------ The test starts generic system interface and generic proxy controller for CanOpen devices. Generic system interface enables integration of values from the CAN Bus into ros2_control framework and the controller enables you to send and receive data from the CAN bus through ros2_control to ROS2. The next few lines show you some command to have exemplary usage of the ros2_control integration: 1. After the test is started check ``/dynamic_joint_states`` to show internal data from CAN nodes in ros2_control system. .. code-block:: bash ros2 topic echo /dynamic_joint_states 2. Open a new terminal and echo data from the topic ``/node_1_controller/rpdo``. .. code-block:: bash ros2 topic echo /node_1_controller/rpdo 3. Open a new terminal reset the state of nmt. .. code-block:: bash ros2 service call /node_1_controller/nmt_reset_node std_srvs/srv/Trigger {} You will expect changes in the ``/dynamic_joint_states`` topic. 4. In a new terminal publish some data to the controller to write them to the CAN bus: .. code-block:: bash ros2 topic pub --once /node_1_controller/tpdo canopen_interfaces/msg/COData " index: 0x4000 subindex: 0 data: 0x1122" Now watch how data in the topic ``/node_1_controller/rpdo`` are changing. There is a mirror of the data on 0x4001. That is, the slave node will mirror the data on 0x4001 via its tpdo and the proxy device will get the data via its rpdo. You should see this .. code-block:: bash index: 16385 # This is 0x4001 subindex: 0 data: 4386 --- ================================================ FILE: canopen/sphinx/user-guide/cia402-driver.rst ================================================ Cia402 Driver ======================== The Cia402 Driver implements the CIA402 profile for motion controllers and enables setting the drive status, operation mode and sending target values to the motion controller. .. csv-table:: CIA402 Drivers :header: Type, Package, Name :widths: 30, 20, 50 lifecycle, canopen_402_driver, ros2_canopen::LifecycleCia402Driver simple, canopen_402_driver, ros2_canopen::Cia402Driver Services -------- .. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left * - Services - Type - Description * - ~/nmt_reset_node - Trigger - Resets CANopen Device the Proxy Device Node manages. * - ~/sdo_read - CORead - Reads an SDO object from the specified index, subindex and datatype of the remote device. * - ~/sdo_write - COWrite - Writes data to an SDO object on the specified index, subindex and datatype of the remote device. * - ~/init - Trigger - Initialises motion controller including referencing * - ~/recover - Trigger - Recovers motion controller * - ~/halt - Trigger - Stops motion controller * - ~/position_mode - Trigger - Switches to profiled position mode * - ~/velocity_mode - Trigger - Switches to profiled velocity mode * - ~/torque_mode - Trigger - Switches to profiled torque mode * - ~/cyclic_position_mode - Trigger - Switches to cyclic position mode * - ~/cyclic_velocity_mode - Trigger - Switches to cyclic velocity mode * - ~/cyclic_torque_mode - Trigger - Switches to cyclic torque mode * - ~/interpolated_position_mode - Trigger - Switches to interpolated position mode, only linear mode with fixed time is supported * - ~/target - CODouble - Sets the target value. Only accepted when an operation mode is set. Publishers ---------- .. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left * - Publishers - Type - Description * - ~/joint_states - sensor_msgs/msg/JointState - Joint states of the drive * - ~/nmt_state - String - Publishes NMT state on change * - ~/rpdo - COData - Publishes received PDO objects on reception Subscribers ----------- .. list-table:: :widths: 30 20 50 :header-rows: 1 * - Topic - Type - Description * - ~/target - COTargetDouble - Sets target value. * - ~/tpdo - COData - Writes received data to remote device if the specified object is RPDO mapped on remote device. Bus Configuration Parameters ---------------------------- Additional parameters that can be used in bus.yml for this driver. .. list-table:: :widths: 30 20 50 :header-rows: 1 * - Parameter - Type - Description * - polling - bool - Enables polling of the drive status. Default: true. If false, period will be used to run a ros2 timer as update loop. If true, the update loop will be triggered by the sync signal and directly executed in the canopen realtime loop. This requires all data processed in the update loop to be PDO, otherwise the loop will get stuck. This can speed reduce processor load significantly though. * - period - Milliseconds - Refresh period for 402 state machine. Should be similar to sync period of master. * - switching_state - see below - The state to switch the operation mode in. * - scale_pos_to_dev - double - Scaling factor to convert from SI units to device units for position. * - offset_pos_to_dev - double - Offset in device units added to scaled position commands sent to the device. * - scale_vel_to_dev - double - Scaling factor to convert from SI units to device units for velocity. * - scale_pos_from_dev - double - Scaling factor to convert from device units to SI units for position. * - offset_pos_from_dev - double - Offset in SI units to added to scaled position reports from the device. * - scale_vel_from_dev - double - Scaling factor to convert from device units to SI units for velocity. * - position_mode - int - The drives operation mode to use for the position interface * - velocity_mode - int - The drives operation mode to use for the velocity interface * - torque_mode - int - The drives operation mode to use for the torque interface ================================================ FILE: canopen/sphinx/user-guide/configuration.rst ================================================ Configuration Package ===================== A configuration package contains one or more configurations for the CANopen stack. The configuration package needs to hold the EDS/DCF files for each device, the bus configuration and the launch files for the different configurations. Consequently, the structure of the configuration package should look as follows: :: {package_name} ├── config │ ├── {bus_config_name_1} │ | ├── bus.yml │ | ├── {device1}.eds │ | ├── {device...}.eds │ | └── {slave_n}.eds │ └── {bus_config_name_2} │ ├── bus.yml │ ├── {device1}.eds │ ├── {device...}.eds │ └── {slave_n}.eds ├── launch │ ├── {bus_config_name_1}.launch.py | └── {bus_config_name_1}.launch.py ├── CMakeLists.txt └── package.xml Bus Configuration File ============================ The ros2_canopen stack relies on a YAML configuration file that is used for configuring the bus topology and specifying configurations for each device. The file details which devices connected to the bus, which EDS/DCF file applies to them, which parameters of the EDS/DCF files should be overwritten and which drivers should be used to control the devices. Structure --------- The YAML configuration file has the following sections: .. code-block:: yaml options: # General options especially dcf_path [configuration item]: [value] master: # The configuration of the master [configuration item]: [value] [...] defaults: # Defaults that apply to all slave nodes [configuration item]: [value] [] nodes: # Configurations for all slave nodes - [device_name]: [configuration item]: [value] [...] Options Section --------------- The options section holds general options. Right now these are only the following. .. csv-table:: Options Configuration :header-rows: 1 :class: longtable :delim: ; :widths: 1 1 configuration item; description dcf_path; The directory in which the generated .bin file will be available at runtime. You can set this to "@BUS_CONFIG_PATH@" and it will be auto-generated by cmake. Master Section -------------- The master section has a number of configuration options. These are not unique to ros2_canopen but come from the lely core library. Below you find a list of possible configuration items. .. csv-table:: Master Configuration :header-rows: 1 :class: longtable :delim: ; :widths: 1 1 configuration item; description node_id; The node-ID (default: 255) driver; The fully qualified class name of the master to use. package; The ros2 package name in which the master class can be found. namespace; The namespace in which the master will be created (default: "/"). baudrate; The baudrate in kbit/s (default: 1000) vendor_id;The vendor-ID (default: 0x00000000) product_code;The product code (default: 0x00000000) revision_number; The revision number (default: 0x00000000). serial_number; The serial number (default: 0x00000000). heartbeat_multiplier; The multiplication factor used to obtain the slave heartbeat consumer time from the master heartbeat producer time (default: see options section). heartbeat_consumer; Specifies whether the master should monitor the heartbeats of the slaves (default: true). heartbeat_producer; The heartbeat producer time in ms (default: 0). emcy_inhibit_time; The EMCY inhibit time in multiples of 100 μs (default: 0, see object 1015). sync_period; The SYNC interval in μs (default: 0). sync_window; The SYNC window length in μs (default: 0, see object 1007). sync_overflow; The SYNC counter overflow value (default: 0, see object 1019). error_behavior; A dictionary of error behaviors for different classes or errors (default: {1: 0x00}, see object 1029). nmt_inhibit_time; The NMT inhibit time in multiples of 100 μs (default: 0, see object 102A). start; Specifies whether the master shall switch into the NMT operational state by itself (default: true, see bit 2 in object 1F80). start_nodes; Specifies whether the master shall start the slaves (default: true, see bit 3 in object 1F80). start_all_nodes; Specifies whether the master shall start all nodes simultaneously (default: false, see bit 1 in object 1F80). reset_all_nodes; Specifies whether all slaves shall be reset in case of an error event on a mandatory slave (default: false, see bit 4 in object 1F80). stop_all_nodes; Specifies whether all slaves shall be stopped in case of an error event on a mandatory slave (default: false, see bit 6 in object 1F80). boot_time; The timeout for booting mandatory slaves in ms (default: 0, see object 1F89). boot_timeout; The timeout for booting all slaves in ms (default: 2000ms). Device Section -------------- The device configuration enables configuring the characteristics of the connected CANopen device. .. note:: It is important to note, that you choose the operation (simple nodes or managed nodes) by choosing either only lifecycle drivers or only simple drivers. **Mixing them will not work!** .. csv-table:: Device Configuration :header-rows: 1 :class: longtable :delim: ; :widths: 1 1 configuration item; description driver; The fully qualified class name of the driver to use. package; The ros2 package name in which the driver class can be found. namespace; The namespace in which the driver will be created (default: "/"). enable_lazy_load; A flag that states whether the driver is loaded on start-up. dcf; The filename of the EDS/DCF describing the slave (mandatory). dcf_path; The directory in which the generated .bin file will be available at runtime (default: see options section). node_id; The node-ID (default: 255, can be omitted if specified in the DCF). revision_number; The revision number (default: 0x00000000, can be omitted if specified in the DCF). serial_number; The serial number (default: 0x00000000, can be omitted if specified in the DCF). heartbeat_multiplier; The multiplication factor used to obtain master heartbeat consumer time from the slave heartbeat producer time (default: see options section). heartbeat_consumer; Specifies whether the slave should monitor the heartbeat of the master (default: false). heartbeat_producer; The heartbeat producer time in ms (default: 0). error_behavior; A dictionary of error behaviors for different classes or errors (default: {}, see object 1029). rpdo; The Receive-PDO configuration (see below). tpdo; The Transmit-PDO configuration (see below). boot; Specifies whether the slave will be configured and booted by the master (default: true, see bit 2 in object 1F81). mandatory; Specifies whether the slave is mandatory (default: false, see bit 3 in object 1F81). reset_communication; Specifies whether the NMT reset communication command may be sent to the slave (default: true, see bit 4 in object 1F81). software_file; The name of the file containing the firmware (default: "", see object 1F58). software_version; The expected software version (default: 0x00000000, see object 1F55). configuration_file; The name of the file containing the configuration (default: "/.bin" (where is the section name), see object 1F22). restore_configuration; The sub-index of object 1011 to be used when restoring the configuration (default: 0x00). sdo_timeout_ms; The timeout to use for SDO reads/writes to this device. (default: 20ms) sdo; Additional SDO requests to be sent during configuration (see below). Further references ------------------ The dcfgen documentation gives more details on the usage of the dcfgen tool for generating DCF: https://opensource.lely.com/canopen/docs/dcf-tools/ Variables --------- ``@BUS_CONFIG_PATH@:`` Automatic config path definition if configuration package structure is followed. Configuration Package CMake =========================== In order to build the configuration package and generate the necessary runtime artifacts from the bus configuration file and eds/dcf files, the lely_core_libraries package contains an extra CMAKE macro. **cogen_dcf(target)** Target: the name of the configuration (e.g. for config/{bus_config_name_1} is bus_config_name_1) .. code-block:: cogen_dcf(bus_config) ================================================ FILE: canopen/sphinx/user-guide/how-to-create-a-cia301-system.rst ================================================ How to create a cia301 system with ros2_control ================================================= The CiA 301 profile, also known as the CANopen application layer, is the foundation of the CANopen protocol. It defines the basic principles, communication services, and object dictionary used for device communication and network management in a CANopen system. The CiA 301 profile defines several communication services that allow devices to exchange data and perform actions. These services include the SDO (Service Data Object) service for transferring object data between devices, the PDO (Process Data Object) service for real-time data exchange, and the NMT (Network Management) service for network initialization, device state control, and error handling. To bring up the CiA301 interface using ROS2, it includes three steps: - preparing the configuration for bus and ``ros2_control`` - preparing the state and command interfaces, - and finally preparing the launch file. Preparing the configuration -------------------------------------------------------- To use the control system interface for CiA301 profile, we should prepare the following - bus_conf - master_dcf - master_bin - can_interface, (default: vcan0) Define the bus configuration parameters .. code-block:: yaml master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" baudrate: 250 options: dcf_path: "@BUS_CONFIG_PATH@" joint_1: node_id: 0x00 dcf: "joint.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" reset_communication: false joint_2: node_id: 0x01 dcf: "joint.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" reset_communication: false Define the ``ros2_control`` parameters .. code-block:: yaml controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster joint_1_controller: type: canopen_ros2_controllers/CanopenProxyController joint_2_controller: type: canopen_ros2_controllers/CanopenProxyController joint_1_controller: ros__parameters: joint: joint_1 joint_2_controller: ros__parameters: joint: joint_2 The example of master_dcf see https://github.com/ros-industrial/ros2_canopen/blob/master/canopen_tests/config/simple/simple.eds Use RPDO to access the current state -------------------------------------------------------- Defining the Joint and CANopen Data Structure The first step is to define a structure that will hold information about your joints and the associated CANopen data. This structure serves as the foundation for your CANopen network, ensuring that all the relevant data is stored and accessible when needed. PDO Index and Subindex Each Process Data Object (PDO) has an index and a subindex. The index acts as a unique identifier for each PDO, differentiating it from other PDOs in the system. The subindex is used to access individual data fields within each PDO as a PDO can contain multiple data fields. Network Management (NMT) Network Management (NMT) is a fundamental service in the CANopen protocol suite. It offers basic device control commands such as start, stop, and reset, and manages the state of devices within the network. For RPDOs, the data are defined using: - "rpdo/index" - "rpdo/subindex" - "rpdo/type" - "rpdo/data" For NMT, we can read the states via: - "nmt/state" Use TPDO to send commands ---------------------------- In order to send commands to hardware devices in a CANopen network, we first need to export the appropriate hardware interfaces. This is a critical step that enables us to effectively control each joint within our network. Registering Transmit Process Data Objects (TPDOs) Similar to how we handle state interfaces, we must register Transmit Process Data Objects (TPDOs) for each joint. These TPDOs are related to the following commands: - "tpdo/index" - "tpdo/subindex" - "tpdo/type" - "tpdo/data" - "tpdo/owns" Network Management (NMT) Commands Beyond this, we have the ability to register commands associated with Network Management (NMT) to control the state of devices within our network. This is important for the smooth operation and control of our devices. The NMT related commands include: - "nmt/reset" - "nmt/reset_fbk" - "nmt/start" - "nmt/start_fbk" These NMT commands not only help in managing the state of devices but also in providing feedback (indicated by "fbk") from the device to the control system after the execution of a command. This feedback mechanism is crucial for ensuring the successful execution of commands and managing the overall health of the network. How to launch the nodes ---------------------------- Finally, we prepare the launch file for the interface. An example see: https://github.com/ros-industrial/ros2_canopen/blob/master/canopen_ros2_control/launch/canopen_system.launch.py For testing, please refer to following section. ================================================ FILE: canopen/sphinx/user-guide/how-to-create-a-configuration.rst ================================================ How to create a configuration package ======================================== In order to use the ros2_canopen stack for your robot, you need to create a configuration package, that holds the configuration of the bus as well as you launch script. The following sections detail the steps to create such a package. Package creation ---------------- When you create your package, you have to first make some decisions. You will need to choose a package name, decide which bus configurations you need to have (usually one per CAN interface) and which slaves you have. | **package_name**: Name of the package | **bus_config_name**: Name of the the bus configuration (you can have multiple) You can use ``ros2 pkg create`` command to create your package. .. code-block:: console $ ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name} $ cd {package_name} $ rm -rf src $ rm -rf include $ mkdir -p launch $ mkdir -p config Now your package directory should look like this. :: {package_name} ├── config ├── launch ├── CMakeLists.txt └── package.xml Bus configuration creation ------------------------------ #. **Bus configuration decisions** Decide how many bus configurations you need. Add one subfolder for each bus configuration in your `config`-folder. .. code-block:: bash $ mkdir -p {bus_config_name} #. **Add device information to bus configurations** Once you created the folders for your bus configurations add the .eds files for the devices that are in the bus configuration to the respective folder. Now your package directory should now look like this. :: {package_name} ├── config │ ├── {bus_config_name_1} │ | ├── {device1}.eds │ | ├── {device...}.eds │ | └── {slave_n}.eds │ └── {bus_config_name_2} │ ├── {device1}.eds │ ├── {device...}.eds │ └── {slave_n}.eds ├── CMakeLists.txt └── package.xml #. **Create the bus configuration specifications** To specify the bus configuration ros2_canopen uses the a YAML-file called bus.yml. Create the file in the respective bus configuration folder. .. code-block:: console $ touch bus.yml :: {package_name} ├── config │ ├── {bus_config_name_1} │ | ├── bus.yml │ | ├── {device1}.eds │ | ├── {device...}.eds │ | └── {slave_n}.eds │ └── {bus_config_name_2} │ ├── bus.yml │ ├── {device1}.eds │ ├── {device...}.eds │ └── {slave_n}.eds ├── CMakeLists.txt └── package.xml #. **Edit the bus configuration specifications** You need to modify each bus.yml file according to your needs. First you need to define where these files and generated files will be found at runtime. This is usually the following if you use colcon to build from source. .. code-block:: yaml options: dcf_path: install/{package_name}/share/{package_name}/config/{bus_config_name} Then you need to define your master. .. code-block:: yaml master: node_id: [node id] package: [ros2 package where to find the master driver (usually canopen_core)] driver: [component type of the driver (ros2_canopen::MasterDriver or ros2_canopen::LifecycleMasterDriver)] Make sure, that you specify a lifecycle master if you use the lifecycled version of ros2_canopen. And add other configuration data as necessary. A documentation of configuration options available can be found in the :doc:`configuration` documentation. Once you have defined the configuration of your master, add your slaves. The following describes the mandatory data per slave. Further configuration options can be found in the :doc:`configuration` documentation. The slave name is the node name that will be assigned to the driver. .. code-block:: yaml nodes: - [unique slave name]: node_id: [node id] package: [ros2 package where to find the driver] driver: [qualified name of the driver] Make sure you use a lifecycle slave if you use the lifecycled version of ros2_canopen. Launch configuration creation ----------------------------- Create a launch folder in your package directory and a launch file. .. code-block:: console mkdir launch touch {...}.launch.py Add the following code: .. code-block:: python def generate_launch_description(): """Generate launch description with multiple components.""" path_file = os.path.dirname(__file__) ld = launch.LaunchDescription() device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("{package_name}"), "config", "{bus_config_name}", "master.dcf", ), "master_bin": os.path.join( get_package_share_directory("{package_name}"), "config", "{bus_config_name}", "master.bin", ), "bus_config": os.path.join( get_package_share_directory("{package_name}"), "config", "{bus_config_name}", "bus.yml", ), "can_interface_name": "{can_interface_name i.e. can0}", }.items(), ) ld.add_action(device_container) return ld CMAKE Configuration creation ----------------------------- Finally we need to adjust the CMakeLists.txt file to pick everything up correctly. .. code-block:: cmake cmake_minimum_required(VERSION 3.8) project({package_name}) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(canopen_base_driver REQUIRED) find_package(canopen_proxy_driver REQUIRED) find_package(lely_core_libraries REQUIRED) cogen_dcf({bus_config_name}) install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/ ) install(DIRECTORY launch_tests/ DESTINATION share/${PROJECT_NAME}/launch_tests/ ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() ament_package() ================================================ FILE: canopen/sphinx/user-guide/how-to-create-a-robot-system.rst ================================================ How to create a robot system with ros2_control ============================================== This guide describes how to create a simple robot using the robot system hardware interface leveragin ros2_control. 1. Create a new configuration package with the name ``canopen_robot_control_example``. .. code-block:: console $ ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name} $ cd {package_name} $ rm -rf src $ rm -rf include $ mkdir -p launch $ mkdir -p config 2. Create a new bus configuration folder with the name ``robot_control``. .. code-block:: console $ mkdir -p config/robot_control 3. Create a new bus configuration file with the name ``bus.yml``. .. code-block:: console $ touch config/robot_control/bus.yml 4. Add the following content to the ``bus.yml`` file. .. code-block:: yaml options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 position_mode: 1 revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} - {index: 0x6060, sub_index: 0, value: 7} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position nodes: joint_1: node_id: 2 joint_2: node_id: 3 5. Copy the ``cia402_slave.eds`` file from the ``canopen_tests/config/robot_control`` package to the ``config/robot_control`` folder. 6. Create a ros2_controllers.yaml and add the following content. .. code-block:: yaml controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster forward_position_controller: type: forward_command_controller/ForwardCommandController forward_position_controller: ros__parameters: joints: - joint1 - joint2 interface_name: position 7. Create a launch file with the name ``robot_control.launch.py`` in the launch directory of your package and add the following content. .. code-block:: python from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [ FindPackageShare("canopen_tests"), "urdf", "robot_controller", "robot_controller.urdf.xacro", ] ), ] ) robot_description = {"robot_description": robot_description_content} robot_control_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/robot_control", "ros2_controllers.yaml"] ) control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_control_config], output="screen", ) joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) forward_position_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) slave_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/robot_control", "cia402_slave.eds"] ) slave_launch = PathJoinSubstitution( [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_config, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_config, }.items(), ) nodes_to_start = [ control_node, joint_state_broadcaster_spawner, forward_position_controller_spawner, robot_state_publisher_node, slave_node_1, slave_node_2 ] return LaunchDescription(nodes_to_start) 8. Create a urdf folder add all files from the ``canopen_tests/urdf/robot_controller`` package to the urdf folder of your package. 9. Edit the CMakeLists.txt file of your package and add the following lines after the find_package section. .. code-block:: cmake cogen_dcf(robot_control) install(DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME}) 10. Build your package and source the setup.bash file. 11. Start your launch file 12. You can now control the robot with the forward_command_controller. You can as well visualize the robot in rviz by adding a tf or a robot model and setting the fixed frame to ``base_link``. You can move the robot with the following command. .. code-block:: bash ros2 topic pub /joint1/forward_position_controller/command std_msgs/msg/Float64 "data: [1.0, 1.0]" ================================================ FILE: canopen/sphinx/user-guide/master.rst ================================================ Master Driver ============= The master driver handles the creation of the necessary can interface and sets up a canopen event loop which drivers can hook onto. In addition, the node offers services for communicating with nodes via nmt and sdo. .. csv-table:: Master Drivers :header: Type, Package, Name :widths: 30, 20, 50 lifecycle, canopen_master_driver, ros2_canopen::LifecycleMasterDriver simple, canopen_master_driver, ros2_canopen::MasterDriver Services -------- .. list-table:: :widths: 30 20 50 :header-rows: 1 * - Services - Type - Description * - ~/read_sdo - COReadID - Reads an SDO object specified by Index, Subindex and Datatype of the device with the specified nodeid. * - ~/write_sdo - COWriteID - Writes Data to an SDO object specified by Index, Subindex and Datatype on the device with the specified nodeid. * - ~/set_nmt - CONmtID - Sends the NMT command to the device with the specified nodeid ================================================ FILE: canopen/sphinx/user-guide/operation/managed-service-interface.rst ================================================ Managed Service Interface ============================ Device Container with managed nodes """"""""""""""""""""""""""""""""""" The device container implements ROS2 component manager. The load and unload services are disabled. Devices are loaded based on the Bus Configuration File (bus.yml). The device container provides the list service, which can be used with ros2cli to check which components have been loaded. .. figure:: ../../images/device-manager.png :alt: Device Manager Concept device manager concept The device container uses the bus description file to identify the correct drivers for each devices. On launch it will load the CANopen master node and driver nodes and pass the appropriate configuration data to the nodes. The nodes are now in unconfigured state. When using the default launch files in canopen_core the lifecycle manager node will automatically be launched. The lifecycle manager takes care of sequencing the lifecycle of the different nodes in the device container. By bringing the lifecycle_manager to active lifecycle state, all master and driver nodes will be activated in correct sequence. If you choose to write your own lifecycle_manager, you'll need to remember, that the master needs to be configured and activated before any driver node can be configured or activated. Bus Configuration """"""""""""""""" The bus configuration for the managed service interface needs to use the driver classes that are marked as lifecycle drivers. The master driver indicates whether the bus.yml will be treated as managed or un-managed service interface. .. csv-table:: Available Driver Components :header: "Package", "Component" canopen_master_driver, ros2_canopen::LifecycleMasterDriver canopen_proxy_driver, ros2_canopen::LifecycleProxyDriver canopen_402_driver, ros2_canopen::LifecycleCia402Driver Launching """"""""""""" The device manager has the following configuration parameters. .. csv-table:: Parameters :header: "Parameter", "Type", "Description" bus_conf, string, (Mandatory) Path to the bus configuration YAML-file master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") can_interface_name, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) ================================================ FILE: canopen/sphinx/user-guide/operation/ros2-control-interface.rst ================================================ Hardware Interface ------------------ This package provides multiple hardware interfaces for testing. Mainly the following: - canopen_ros2_control/CanopenSystem: A system interface for ProxyDrivers - canopen_ros2_control/Cia402System: A system interface for Cia402Drivers - canopen_ros2_control/Cia402RobotSystem: A system interface for Cia402Drivers in a robot configuration (under development) Robot System Interface '''''''''''''''''''''' The robot system interface takes a number of inputs from the robot description (urdf). It will make the Cia402Drivers available via the ros2_control hardware interface. The bus has to still be defined in the bus.yml file. In the urdf you can the choose the CANopen nodes that have a Cia402Driver attached to them. The ros2_control interface only works with non-lifecycle drivers right now. For each joint in your urdf you can choose the attached CANopen device by using the ``node_id`` parameter. The ``node_id`` parameter is the CANopen node id of the device. .. code-block:: xml canopen_ros2_control/Cia402RobotSystem [path to bus.yml] [path to master.dcf] [can interface to be used] [master.bin if it exists] 3 ... 3 ... .. note:: You can find an example for the configuration in the ``canopen_tests`` package under robot_control. ROS2 Controllers ---------------- This package provides multiple controllers for testing. Mainly the following: - canopen_ros2_controllers/Cia402RobotController: Works with Robot System Interface - canopen_ros2_controllers/Cia402DeviceController: Works with Cia402System - canopen_ros2_controllers/CanopenProxyController: Works with CanopenSystem and Cia402System Robot Controller '''''''''''''''' The robot controller enables bringing up the different joints of the robot automatically by using the ros2_controller lifecycle. There is no need for further action, once the controller is activated, the drives are ready to be used. The robot controller can be configured in the ros2_controllers.yaml with the following parameters: .. code-block:: yaml robot_controller: ros__parameters: joints: # joints that are controlled by the controller - joint1 - joint2 operation_mode: 1 # operation mode of the controller command_poll_freq: 5 # frequency with which the controller polls for command feedback ================================================ FILE: canopen/sphinx/user-guide/operation/service-interface.rst ================================================ Service Interface ================== Device Container """"""""""""""""" The device container implements ROS2 component manager. The load and unload services are disabled. Devices are loaded based on the Bus Configuration File (bus.yml). It provides the list service though. .. figure:: ../../images/device-manager.png :alt: Device Manager Concept device manager concept The device manager uses the bus description file to identify the correct drivers for each devices. On launch it will load the CANopen master node and pass the generated DCF files to configure the CANopen master correctly for you bus configuration. It will the enable the master. Once the master is enabled it will sequentially load and enable all drivers in the bus configuration. Once a CANopen Node comes online (i.e. sends the boot indication) the CANopen master will configure the node with the parameters and commands specified in the bus configuration for that device. When the configuration of the device is done, all data send by the device is forwarded to the appropriate driver. All loaded nodes are added to the device manager's executor. .. figure:: ../../images/device-manager-usage.png :alt: Device Manager Usage device manager usage Bus Configuration """"""""""""""""" The bus configuration for the needs to use the driver classes that are marked as non lifecycle drivers. .. csv-table:: Available Driver Components :header: "Package", "Component" canopen_core, ros2_canopen::MasterDriver canopen_proxy_driver, ros2_canopen::ProxyDriver canopen_402_driver, ros2_canopen::Cia402Driver Launching """"""""""""" The device manager has the following configuration parameters. .. csv-table:: Parameters :header: "Parameter", "Type", "Description" bus_conf, string, (Mandatory) Path to the bus configuration YAML-file master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") can_interface_name, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) ================================================ FILE: canopen/sphinx/user-guide/operation.rst ================================================ Operation ========= The ros2_canopen stack can be used in three different ways: * standard nodes container * managed nodes container * ros2_control system interface with standard nodes Simple nodes container """""""""""""""""""""""" The standard node container mode bundles the master and all slave driver nodes in one specialised container called device container. All nodes are simple ROS 2 nodes and expose a publish and subscribe as well as a service interfaces. Once the device container is started, all nodes are brought up and ready to be used. **Purpose**: The simple nodes container is thought for applications where the user needs a simple and easy to launch interface and does not need any realtime control capabilities as provided by ros2_control. Managed nodes container """""""""""""""""""""""""" The managed nodes container has the same properties as the standard nodes container. The exception is, that all nodes are lifecycle nodes and there is a special node called lifecycle manager. The user can use the lifecycle manager to control the lifecycle of all nodes in the container. **Purpose**: The managed nodes container is thought for applications where the user wants to have more runtime recovery options than killing and restarting the container. ROS 2 control system interface """""""""""""""""""""""""""""" The ros2_control interface is currently build on top of the simple nodes container. In addition to the standard nodes container the ros2_control system interface provides a hardware interface that can be used to control the devices on the bus. Currently, three different system interfaces are provided: * canopen_ros2_control/CANopenSystem * canopen_ros2_control/CIA402System * canopen_ros2_control/RobotSystem **Purpose**: The ROS 2 control system interfaces are thought for control applications that require low latencies. ================================================ FILE: canopen/sphinx/user-guide/proxy-driver.rst ================================================ Proxy Driver =================== A proxy driver which simply forwards CANopen functionality for a specific device via ROS2 services and messages. .. csv-table:: Proxy Drivers :header: Type, Package, Name :widths: 30, 20, 50 lifecycle, canopen_proxy_driver, ros2_canopen::LifecycleProxyDriver simple, canopen_proxy_driver, ros2_canopen::ProxyDriver Services -------- .. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left * - Services - Type - Description * - ~/nmt_reset_node - Trigger - Resets CANopen Device the Proxy Device Node manages. * - ~/sdo_read - CORead - Reads an SDO object from the specified index, subindex and datatype of the remote device. * - ~/sdo_write - COWrite - Writes data to an SDO object on the specified index, subindex and datatype of the remote device. Publishers ---------- .. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left * - Topic - Type - Description * - ~/nmt_state - String - Publishes NMT state on change * - ~/rpdo - COData - Publishes received PDO objects on reception Subscribers ----------- .. list-table:: :widths: 30 20 50 :header-rows: 1 * - Topic - Type - Description * - ~/tpdo - COData - Writes received data to remote device if the specified object is RPDO mapped on remote device. ================================================ FILE: canopen_402_driver/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * Fix configuration parsing and logging * add pdo 6077 torque actual value to the joint state interface as effort (`#316 `_) * Contributors: ipa-vsp, synsi23b 0.3.1 (2025-06-23) ------------------ * Homing timeout * Add services to disable/enable motor so that brake is usable. * Contributors: Vishnuprasad Prachandabhanu 0.3.0 (2024-12-12) ------------------ * Reformat using pre-commit * Implement position offsets * pre-commit fix * impl operation mode * Add cyclic torque mode to cia402 driver and robot system controller (`#293 `_) * Add base functions for switching to cyclic torque mode * Add cyclic torque mode as effort interface to robot_system controller * Add documentation about cyclic torque mode. Co-authored-by: Christoph Hellmann Santos 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Merge pull request `#267 `_ from clalancette/clalancette/update-lely-core-hash Update the lely_core_libraries hash to the latest. * fix ci build error * Contributors: Vishnuprasad Prachandabhanu, ipa-vsp 0.2.9 (2024-04-16) ------------------ * Update the lely_core_libraries hash to the latest. * fix ci build error * Contributors: Vishnuprasad Prachandabhanu 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix boost/std placeholders ambiguity in older boost versions * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, G.A. vd. Hoorn, Lovro, Vishnuprasad Prachandabhanu, livanov93 ================================================ FILE: canopen_402_driver/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_402_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(canopen_base_driver REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(canopen_proxy_driver REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) add_library(lely_motion_controller_bridge src/motor.cpp src/command.cpp src/state.cpp src/default_homing_mode.cpp ) target_compile_features(lely_motion_controller_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lely_motion_controller_bridge PUBLIC -Wl,--no-undefined) target_include_directories(lely_motion_controller_bridge PUBLIC $ $) target_link_libraries(lely_motion_controller_bridge PUBLIC canopen_base_driver::node_canopen_base_driver rclcpp::rclcpp ) add_library(node_canopen_cia402_driver src/node_interfaces/node_canopen_402_driver.cpp ) target_compile_features(node_canopen_cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_cia402_driver PUBLIC -Wl,--no-undefined) target_include_directories(node_canopen_cia402_driver PUBLIC $ $) target_link_libraries(node_canopen_cia402_driver PUBLIC ${canopen_interfaces_TARGETS} ${sensor_msgs_TARGETS} canopen_proxy_driver::node_canopen_proxy_driver lely_motion_controller_bridge ) add_library(lifecycle_cia402_driver src/lifecycle_cia402_driver.cpp ) target_compile_features(lifecycle_cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_cia402_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_cia402_driver PUBLIC $ $) target_link_libraries(lifecycle_cia402_driver PUBLIC canopen_core::node_canopen_driver node_canopen_cia402_driver rclcpp_components::component rclcpp_lifecycle::rclcpp_lifecycle ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(lifecycle_cia402_driver PRIVATE "CANOPEN_402_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(lifecycle_cia402_driver "ros2_canopen::LifecycleCia402Driver") set(node_plugins "${node_plugins}ros2_canopen::LifecycleCia402Driver;$\n") add_library(cia402_driver src/cia402_driver.cpp ) target_compile_features(cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(cia402_driver PUBLIC -Wl,--no-undefined) target_include_directories(cia402_driver PUBLIC $ $) target_link_libraries(cia402_driver PUBLIC canopen_core::node_canopen_driver node_canopen_cia402_driver rclcpp_components::component ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(cia402_driver PRIVATE "CANOPEN_cia402_driver_BUILDING_LIBRARY") rclcpp_components_register_nodes(cia402_driver "ros2_canopen::Cia402Driver") set(node_plugins "${node_plugins}ros2_canopen::Cia402Driver;$\n") install( DIRECTORY include/ DESTINATION include ) install( TARGETS lifecycle_cia402_driver cia402_driver node_canopen_cia402_driver lely_motion_controller_bridge EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( lifecycle_cia402_driver cia402_driver node_canopen_cia402_driver lely_motion_controller_bridge ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( canopen_base_driver canopen_core canopen_interfaces canopen_proxy_driver rclcpp rclcpp_components rclcpp_lifecycle sensor_msgs ) ament_package() ================================================ FILE: canopen_402_driver/LICENSE ================================================ GNU LESSER GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. This version of the GNU Lesser General Public License incorporates the terms and conditions of version 3 of the GNU General Public License, supplemented by the additional permissions listed below. 0. Additional Definitions. As used herein, "this License" refers to version 3 of the GNU Lesser General Public License, and the "GNU GPL" refers to version 3 of the GNU General Public License. "The Library" refers to a covered work governed by this License, other than an Application or a Combined Work as defined below. An "Application" is any work that makes use of an interface provided by the Library, but which is not otherwise based on the Library. Defining a subclass of a class defined by the Library is deemed a mode of using an interface provided by the Library. A "Combined Work" is a work produced by combining or linking an Application with the Library. The particular version of the Library with which the Combined Work was made is also called the "Linked Version". The "Minimal Corresponding Source" for a Combined Work means the Corresponding Source for the Combined Work, excluding any source code for portions of the Combined Work that, considered in isolation, are based on the Application, and not on the Linked Version. The "Corresponding Application Code" for a Combined Work means the object code and/or source code for the Application, including any data and utility programs needed for reproducing the Combined Work from the Application, but excluding the System Libraries of the Combined Work. 1. Exception to Section 3 of the GNU GPL. You may convey a covered work under sections 3 and 4 of this License without being bound by section 3 of the GNU GPL. 2. Conveying Modified Versions. If you modify a copy of the Library, and, in your modifications, a facility refers to a function or data to be supplied by an Application that uses the facility (other than as an argument passed when the facility is invoked), then you may convey a copy of the modified version: a) under this License, provided that you make a good faith effort to ensure that, in the event an Application does not supply the function or data, the facility still operates, and performs whatever part of its purpose remains meaningful, or b) under the GNU GPL, with none of the additional permissions of this License applicable to that copy. 3. Object Code Incorporating Material from Library Header Files. The object code form of an Application may incorporate material from a header file that is part of the Library. You may convey such object code under terms of your choice, provided that, if the incorporated material is not limited to numerical parameters, data structure layouts and accessors, or small macros, inline functions and templates (ten or fewer lines in length), you do both of the following: a) Give prominent notice with each copy of the object code that the Library is used in it and that the Library and its use are covered by this License. b) Accompany the object code with a copy of the GNU GPL and this license document. 4. Combined Works. You may convey a Combined Work under terms of your choice that, taken together, effectively do not restrict modification of the portions of the Library contained in the Combined Work and reverse engineering for debugging such modifications, if you also do each of the following: a) Give prominent notice with each copy of the Combined Work that the Library is used in it and that the Library and its use are covered by this License. b) Accompany the Combined Work with a copy of the GNU GPL and this license document. c) For a Combined Work that displays copyright notices during execution, include the copyright notice for the Library among these notices, as well as a reference directing the user to the copies of the GNU GPL and this license document. d) Do one of the following: 0) Convey the Minimal Corresponding Source under the terms of this License, and the Corresponding Application Code in a form suitable for, and under terms that permit, the user to recombine or relink the Application with a modified version of the Linked Version to produce a modified Combined Work, in the manner specified by section 6 of the GNU GPL for conveying Corresponding Source. 1) Use a suitable shared library mechanism for linking with the Library. A suitable mechanism is one that (a) uses at run time a copy of the Library already present on the user's computer system, and (b) will operate properly with a modified version of the Library that is interface-compatible with the Linked Version. e) Provide Installation Information, but only if you would otherwise be required to provide such information under section 6 of the GNU GPL, and only to the extent that such information is necessary to install and execute a modified version of the Combined Work produced by recombining or relinking the Application with a modified version of the Linked Version. (If you use option 4d0, the Installation Information must accompany the Minimal Corresponding Source and Corresponding Application Code. If you use option 4d1, you must provide the Installation Information in the manner specified by section 6 of the GNU GPL for conveying Corresponding Source.) 5. Combined Libraries. You may place library facilities that are a work based on the Library side by side in a single library together with other library facilities that are not Applications and are not covered by this License, and convey such a combined library under terms of your choice, if you do both of the following: a) Accompany the combined library with a copy of the same work based on the Library, uncombined with any other library facilities, conveyed under the terms of this License. b) Give prominent notice with the combined library that part of it is a work based on the Library, and explaining where to find the accompanying uncombined form of the same work. 6. Revised Versions of the GNU Lesser General Public License. The Free Software Foundation may publish revised and/or new versions of the GNU Lesser General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Library as you received it specifies that a certain numbered version of the GNU Lesser General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that published version or of any later version published by the Free Software Foundation. If the Library as you received it does not specify a version number of the GNU Lesser General Public License, you may choose any version of the GNU Lesser General Public License ever published by the Free Software Foundation. If the Library as you received it specifies that a proxy can decide whether future versions of the GNU Lesser General Public License shall apply, that proxy's public statement of acceptance of any version is permanent authorization for you to choose that version for the Library. ================================================ FILE: canopen_402_driver/include/canopen_402_driver/base.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_BASE_H #define CANOPEN_402_BASE_H #include #include "lely/coapp/driver.hpp" #include "lely/coapp/master.hpp" namespace ros2_canopen { /** * @brief Motor Base Class * */ class MotorBase { protected: MotorBase() {} public: enum OperationMode { No_Mode = 0, Profiled_Position = 1, Velocity = 2, Profiled_Velocity = 3, Profiled_Torque = 4, Reserved = 5, Homing = 6, Interpolated_Position = 7, Cyclic_Synchronous_Position = 8, Cyclic_Synchronous_Velocity = 9, Cyclic_Synchronous_Torque = 10, }; /** * @brief Set target * * @param [in] val Target value * @return true * @return false */ virtual bool setTarget(double val) = 0; /** * @brief Enter Operation Mode * * @param [in] mode Target Mode * @return true * @return false */ virtual bool enterModeAndWait(uint16_t mode) = 0; /** * @brief Check if Operation Mode is supported * * @param [in] mode Operation Mode to be checked * @return true * @return false */ virtual bool isModeSupported(uint16_t mode) = 0; /** * @brief Get current Mode * * @return uint16_t */ virtual uint16_t getMode() = 0; /** * @brief Register default Operation Modes * */ virtual void registerDefaultModes() {} typedef std::shared_ptr MotorBaseSharedPtr; }; typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr; } // namespace ros2_canopen #endif ================================================ FILE: canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER__402_DRIVER_HPP_ #define CANOPEN_402_DRIVER__402_DRIVER_HPP_ #include #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "canopen_core/driver_node.hpp" namespace ros2_canopen { /** * @brief Abstract Class for a CANopen Device Node * * This class provides the base functionality for creating a * CANopen device node. It provides callbacks for nmt and rpdo. */ class Cia402Driver : public ros2_canopen::CanopenDriver { std::shared_ptr> node_canopen_402_driver_; public: Cia402Driver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); virtual bool reset_node_nmt_command() { return node_canopen_402_driver_->reset_node_nmt_command(); } virtual bool start_node_nmt_command() { return node_canopen_402_driver_->start_node_nmt_command(); } virtual bool tpdo_transmit(ros2_canopen::COData & data) { return node_canopen_402_driver_->tpdo_transmit(data); } virtual bool sdo_write(ros2_canopen::COData & data) { return node_canopen_402_driver_->sdo_write(data); } virtual bool sdo_read(ros2_canopen::COData & data) { return node_canopen_402_driver_->sdo_read(data); } void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_402_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_402_driver_->register_rpdo_cb(rpdo_cb); } double get_effort(uint8_t channel = 0) { return node_canopen_402_driver_->get_effort(channel); } double get_speed(uint8_t channel = 0) { return node_canopen_402_driver_->get_speed(channel); } double get_position(uint8_t channel = 0) { return node_canopen_402_driver_->get_position(channel); } bool set_target(double target, uint8_t channel = 0) { return node_canopen_402_driver_->set_target(target, channel); } bool init_motor(uint8_t channel = 0) { return node_canopen_402_driver_->init_motor(channel); } bool recover_motor(uint8_t channel = 0) { return node_canopen_402_driver_->recover_motor(channel); } bool halt_motor(uint8_t channel = 0) { return node_canopen_402_driver_->halt_motor(channel); } uint16_t get_mode(uint8_t channel = 0) { return node_canopen_402_driver_->get_mode(channel); } bool set_operation_mode(uint16_t mode, uint8_t channel = 0) { return node_canopen_402_driver_->set_operation_mode(mode, channel); } }; } // namespace ros2_canopen #endif // CANOPEN_402_DRIVER__CANOPEN_402_DRIVER_HPP_ ================================================ FILE: canopen_402_driver/include/canopen_402_driver/command.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER_COMMAND_HPP #define CANOPEN_402_DRIVER_COMMAND_HPP #include #include #include #include "state.hpp" namespace ros2_canopen { class Command402 { struct Op { uint16_t to_set_; uint16_t to_reset_; Op(uint16_t to_set, uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {} void operator()(uint16_t & val) const { val = (val & ~to_reset_) | to_set_; } }; class TransitionTable { boost::container::flat_map, Op> transitions_; void add(const State402::InternalState & from, const State402::InternalState & to, Op op) { transitions_.insert(std::make_pair(std::make_pair(from, to), op)); } public: TransitionTable(); const Op & get(const State402::InternalState & from, const State402::InternalState & to) const { return transitions_.at(std::make_pair(from, to)); } }; static const TransitionTable transitions_; static State402::InternalState nextStateForEnabling(State402::InternalState state); Command402(); public: enum ControlWord { CW_Switch_On = 0, CW_Enable_Voltage = 1, CW_Quick_Stop = 2, CW_Enable_Operation = 3, CW_Operation_mode_specific0 = 4, CW_Operation_mode_specific1 = 5, CW_Operation_mode_specific2 = 6, CW_Fault_Reset = 7, CW_Halt = 8, CW_Operation_mode_specific3 = 9, // CW_Reserved1=10, CW_Manufacturer_specific0 = 11, CW_Manufacturer_specific1 = 12, CW_Manufacturer_specific2 = 13, CW_Manufacturer_specific3 = 14, CW_Manufacturer_specific4 = 15, }; static bool setTransition( uint16_t & cw, const State402::InternalState & from, const State402::InternalState & to, State402::InternalState * next); }; } // namespace ros2_canopen #endif // CANOPEN_402_DRIVER_COMMAND_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef DEFAULT_HOMING_MODE_HPP #define DEFAULT_HOMING_MODE_HPP #include #include "canopen_base_driver/lely_driver_bridge.hpp" #include "homing_mode.hpp" namespace ros2_canopen { class DefaultHomingMode : public HomingMode { const uint16_t base_index = 0x6098; uint16_t channel_offset_; // Channel offset for multi-axis support (CiA 402-2) std::shared_ptr driver; std::atomic execute_; std::mutex mutex_; std::condition_variable cond_; uint16_t status_; const int homing_timeout_seconds_; enum SW_masks { MASK_Reached = (1 << State402::SW_Target_reached), MASK_Attained = (1 << SW_Attained), MASK_Error = (1 << SW_Error), }; bool error(const std::string & msg) { execute_ = false; std::cout << msg << std::endl; return false; } public: DefaultHomingMode( std::shared_ptr driver, int homing_timeout_seconds, uint16_t channel_offset = 0) : homing_timeout_seconds_(homing_timeout_seconds), channel_offset_(channel_offset) { this->driver = driver; } virtual bool start(); virtual bool read(const uint16_t & sw); virtual bool write(OpModeAccesser & cw); virtual bool executeHoming(); }; } // namespace ros2_canopen #endif // DEFAULT_HOMING_MODE_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/homing_mode.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef HOMING_MODE_HPP #define HOMING_MODE_HPP #include "base.hpp" #include "mode.hpp" namespace ros2_canopen { class HomingMode : public Mode { protected: enum SW_bits { SW_Attained = State402::SW_Operation_mode_specific0, SW_Error = State402::SW_Operation_mode_specific1, }; enum CW_bits { CW_StartHoming = Command402::CW_Operation_mode_specific0, }; public: HomingMode() : Mode(MotorBase::Homing) {} virtual bool executeHoming() = 0; }; } // namespace ros2_canopen #endif // HOMING_MODE_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ #define CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "canopen_core/driver_node.hpp" namespace ros2_canopen { /** * @brief Lifecycle 402 Driver * * A very basic driver without any functionality. * */ class LifecycleCia402Driver : public ros2_canopen::LifecycleCanopenDriver { std::shared_ptr> node_canopen_402_driver_; public: LifecycleCia402Driver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); virtual bool reset_node_nmt_command() { return node_canopen_402_driver_->reset_node_nmt_command(); } virtual bool start_node_nmt_command() { return node_canopen_402_driver_->start_node_nmt_command(); } virtual bool tpdo_transmit(ros2_canopen::COData & data) { return node_canopen_402_driver_->tpdo_transmit(data); } virtual bool sdo_write(ros2_canopen::COData & data) { return node_canopen_402_driver_->sdo_write(data); } virtual bool sdo_read(ros2_canopen::COData & data) { return node_canopen_402_driver_->sdo_read(data); } void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_402_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_402_driver_->register_rpdo_cb(rpdo_cb); } double get_speed() { return node_canopen_402_driver_->get_speed(); } double get_position() { return node_canopen_402_driver_->get_position(); } bool set_target(double target) { return node_canopen_402_driver_->set_target(target); } bool init_motor() { return node_canopen_402_driver_->init_motor(); } bool recover_motor() { return node_canopen_402_driver_->recover_motor(); } bool halt_motor() { return node_canopen_402_driver_->halt_motor(); } uint16_t get_mode() { return node_canopen_402_driver_->get_mode(); } bool set_operation_mode(uint16_t mode) { return node_canopen_402_driver_->set_operation_mode(mode); } }; } // namespace ros2_canopen #endif // CANOPEN_402_DRIVER__CANOPEN_402_DRIVER_HPP_ ================================================ FILE: canopen_402_driver/include/canopen_402_driver/mode.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER_MODE_HPP #define CANOPEN_402_DRIVER_MODE_HPP #include #include #include "command.hpp" #include "state.hpp" #include "word_accessor.hpp" namespace ros2_canopen { class Mode { public: const uint16_t mode_id_; Mode(uint16_t id) : mode_id_(id) {} typedef WordAccessor< (1 << Command402::CW_Operation_mode_specific0) | (1 << Command402::CW_Operation_mode_specific1) | (1 << Command402::CW_Operation_mode_specific2) | (1 << Command402::CW_Operation_mode_specific3)> OpModeAccesser; virtual bool start() = 0; virtual bool read(const uint16_t & sw) = 0; virtual bool write(OpModeAccesser & cw) = 0; virtual bool setTarget(const double & val) { return false; } virtual ~Mode() {} }; typedef std::shared_ptr ModeSharedPtr; } // namespace ros2_canopen #endif // CANOPEN_402_DRIVER_MODE_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef MODE_FORWARD_HELPER_HPP #define MODE_FORWARD_HELPER_HPP #include #include #include "canopen_base_driver/lely_driver_bridge.hpp" #include "mode_target_helper.hpp" namespace ros2_canopen { template class ModeForwardHelper : public ModeTargetHelper { std::shared_ptr driver; uint16_t channel_offset_; // Channel offset for multi-axis support (CiA 402-2) public: ModeForwardHelper(std::shared_ptr driver, uint16_t channel_offset = 0) : ModeTargetHelper(ID), channel_offset_(channel_offset) { this->driver = driver; } virtual bool read(const uint16_t & sw) { return true; } virtual bool write(Mode::OpModeAccesser & cw) { if (this->hasTarget()) { cw = cw.get() | CW_MASK; // Apply channel offset to object dictionary index driver->universal_set_value(OBJ + channel_offset_, SUB, this->getTarget()); return true; } else { cw = cw.get() & ~CW_MASK; return false; } } }; } // namespace ros2_canopen #endif // MODE_FORWARD_HELPER_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef MODE_TARGET_HELPER_HPP #define MODE_TARGET_HELPER_HPP #include #include #include #include #include #include #include #include #include "canopen_402_driver/mode.hpp" namespace ros2_canopen { template class ModeTargetHelper : public Mode { T target_; std::atomic has_target_; public: ModeTargetHelper(uint16_t mode) : Mode(mode) {} bool hasTarget() { return has_target_; } T getTarget() { return target_; } virtual bool setTarget(const double & val) { if (std::isnan(val)) { // std::cout << "canopen_402 target command is not a number" << std::endl; RCLCPP_DEBUG(rclcpp::get_logger("canopen_402_target"), "Target command is not a number"); return false; } using boost::numeric_cast; using boost::numeric::negative_overflow; using boost::numeric::positive_overflow; try { target_ = numeric_cast(val); } catch (negative_overflow &) { std::cout << "canopen_402 Command " << val << " does not fit into target, clamping to min limit" << std::endl; target_ = std::numeric_limits::min(); } catch (positive_overflow &) { std::cout << "canopen_402 Command " << val << " does not fit into target, clamping to max limit" << std::endl; target_ = std::numeric_limits::max(); } catch (...) { std::cout << "canopen_402 Was not able to cast command " << val << std::endl; return false; } has_target_ = true; return true; } virtual bool start() { has_target_ = false; return true; } }; } // namespace ros2_canopen #endif // MODE_TARGET_HELPER_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/motor.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef MOTOR_HPP #define MOTOR_HPP #include #include #include #include #include #include #include #include #include #include "rclcpp/rclcpp.hpp" #include "canopen_402_driver/default_homing_mode.hpp" #include "canopen_402_driver/mode_forward_helper.hpp" #include "canopen_402_driver/profiled_position_mode.hpp" #include "canopen_base_driver/diagnostic_collector.hpp" #include "canopen_base_driver/lely_driver_bridge.hpp" namespace ros2_canopen { typedef ModeForwardHelper ProfiledVelocityMode; typedef ModeForwardHelper ProfiledTorqueMode; typedef ModeForwardHelper CyclicSynchronousPositionMode; typedef ModeForwardHelper CyclicSynchronousVelocityMode; typedef ModeForwardHelper CyclicSynchronousTorqueMode; typedef ModeForwardHelper< MotorBase::Velocity, int16_t, 0x6042, 0, (1 << Command402::CW_Operation_mode_specific0) | (1 << Command402::CW_Operation_mode_specific1) | (1 << Command402::CW_Operation_mode_specific2)> VelocityMode; typedef ModeForwardHelper< MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, (1 << Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode; class Motor402 : public MotorBase { public: Motor402( std::shared_ptr driver, ros2_canopen::State402::InternalState switching_state, int homing_timeout_seconds, uint8_t channel = 0) : MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5), homing_timeout_seconds_(homing_timeout_seconds), channel_(channel), effort_support_state_(-1) { this->driver = driver; // Calculate channel offset according to CiA 402-2: base_index + (channel * 0x800) channel_offset_ = channel * 0x800; } /** * @brief Get channel-specific object dictionary index * * According to CiA 402-2, multi-axis devices use offset indices: * - Channel 0: 0x6000-0x67FF (standard, no offset) * - Channel 1: 0x6800-0x6FFF (offset +0x800) * - Channel 2: 0x7000-0x77FF (offset +0x1000) * - etc. * * @param base_index The base object dictionary index * @return The channel-specific index */ uint16_t get_channel_index(uint16_t base_index) const { return base_index + channel_offset_; } uint8_t get_channel() const { return channel_; } virtual bool setTarget(double val); virtual bool enterModeAndWait(uint16_t mode); virtual bool isModeSupported(uint16_t mode); virtual uint16_t getMode(); bool readState(); /** * @brief Updates the device diagnostic information * * This function updates the diagnostic information of the device by updating the diagnostic * status message * @ref diagnostic_status_ and publishing it. */ void handleDiag(); /** * @brief Initialise the drive * * This function initialises the drive. This means, it first * attempts to bring the device to operational state (CIA402) * and then executes the chosen homing method. * */ bool handleInit(); /** * @brief Read objects of the drive * * This function should be called regularly. It reads the status word * from the device and translates it into the devices state. * */ void handleRead(); /** * @brief Writes objects to the drive * * This function should be called regularly. It writes the new command * word to the drive * */ void handleWrite(); /** * @brief Shutdowns the drive * * This function shuts down the drive by bringing it into * SwitchOn disabled state. * */ bool handleShutdown(); /** * @brief Executes a quickstop * * The function executes a quickstop. * */ bool handleHalt(); /** * @brief Recovers the device from fault * * This function tries to reset faults and * put the device back to operational state. * */ bool handleRecover(); /** * @brief Enable the drive * * This function enables the drive. This means it attempts * to bring the device to operational state (CIA402), and does nothing else. * */ bool handleEnable(); /** * @brief Disable the drive * * This function disables the drive. This means it attempts to bring the * device to switched on disabled state (CIA402). * */ bool handleDisable(); /** * @brief Register a new operation mode for the drive * * This function will register an operation mode for the drive. * It will check if the mode is supported by the drive by reading * 0x6508 object. * * @tparam T * @tparam Args * @param mode * @param args * @return true * @return false */ template bool registerMode(uint16_t mode, Args &&... args) { return mode_allocators_ .insert(std::make_pair( mode, [args..., mode, this]() { if (isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...))); })) .second; } /** * @brief Tries to register the standard operation modes defined in cia402 * */ virtual void registerDefaultModes() { // Pass channel_offset_ to all mode constructors for multi-axis support registerMode(MotorBase::Profiled_Position, driver, channel_offset_); registerMode(MotorBase::Velocity, driver, channel_offset_); registerMode(MotorBase::Profiled_Velocity, driver, channel_offset_); registerMode(MotorBase::Profiled_Torque, driver, channel_offset_); registerMode( MotorBase::Homing, driver, homing_timeout_seconds_, channel_offset_); registerMode( MotorBase::Interpolated_Position, driver, channel_offset_); registerMode( MotorBase::Cyclic_Synchronous_Position, driver, channel_offset_); registerMode( MotorBase::Cyclic_Synchronous_Velocity, driver, channel_offset_); registerMode( MotorBase::Cyclic_Synchronous_Torque, driver, channel_offset_); } double get_effort() const { auto idx = get_channel_index(0x6077); auto state = effort_support_state_.load(); if (state <= 0) { if (state == 0) { return 0.0; } bool has_object = this->driver->has_object(idx, 0); effort_support_state_.store(has_object ? 1 : 0); if (!has_object) { RCLCPP_WARN( rclcpp::get_logger("canopen_402_driver"), "Effort interface disabled: object 0x6077:00 not found in EDS/OD."); return 0.0; } } return (double)this->driver->universal_get_value(idx, 0); } double get_speed() const { return (double)this->driver->universal_get_value(get_channel_index(0x606C), 0); } double get_position() const { return (double)this->driver->universal_get_value(get_channel_index(0x6064), 0); } void set_diagnostic_status_msgs(std::shared_ptr status, bool enable) { this->enable_diagnostics_.store(enable); this->diag_collector_ = status; } private: virtual bool isModeSupportedByDevice(uint16_t mode); void registerMode(uint16_t id, const ModeSharedPtr & m); ModeSharedPtr allocMode(uint16_t mode); bool switchMode(uint16_t mode); bool switchState(const State402::InternalState & target); std::atomic status_word_; uint16_t control_word_; std::mutex cw_mutex_; std::atomic start_fault_reset_; std::atomic target_state_; State402 state_handler_; std::mutex map_mutex_; std::unordered_map modes_; typedef std::function AllocFuncType; std::unordered_map mode_allocators_; ModeSharedPtr selected_mode_; uint16_t mode_id_; std::condition_variable mode_cond_; std::mutex mode_mutex_; const State402::InternalState switching_state_; const bool monitor_mode_; const std::chrono::seconds state_switch_timeout_; const int homing_timeout_seconds_; std::shared_ptr driver; // Channel support for multi-axis devices (CiA 402-2) const uint8_t channel_; uint16_t channel_offset_; // Base object dictionary indices (will be offset by channel_offset_) const uint16_t status_word_entry_index = 0x6041; const uint16_t control_word_entry_index = 0x6040; const uint16_t op_mode_display_index = 0x6061; const uint16_t op_mode_index = 0x6060; const uint16_t supported_drive_modes_index = 0x6502; // Diagnostic components std::atomic enable_diagnostics_; std::shared_ptr diag_collector_; // -1 unknown, 0 missing, 1 present mutable std::atomic effort_support_state_; }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Vishnuprasad Prachandabhanu // Lovro Ivanov // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef NODE_CANOPEN_402_DRIVER #define NODE_CANOPEN_402_DRIVER #include #include #include #include #include #include "canopen_402_driver/motor.hpp" #include "canopen_base_driver/lely_driver_bridge.hpp" #include "canopen_interfaces/srv/co_target_double.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" #include "sensor_msgs/msg/joint_state.hpp" namespace ros2_canopen { namespace node_interfaces { template class NodeCanopen402Driver : public NodeCanopenProxyDriver { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: struct ChannelContext { std::shared_ptr motor; // Resolved (effective) per-channel scale/offset values double scale_pos_to_dev{1000.0}; double scale_pos_from_dev{0.001}; double scale_vel_to_dev{1000.0}; double scale_vel_from_dev{0.001}; double scale_eff_from_dev{0.001}; double offset_pos_to_dev{0.0}; double offset_pos_from_dev{0.0}; // Per-channel service handles (kept alive by owning SharedPtr) rclcpp::Service::SharedPtr handle_init; rclcpp::Service::SharedPtr handle_enable; rclcpp::Service::SharedPtr handle_disable; rclcpp::Service::SharedPtr handle_halt; rclcpp::Service::SharedPtr handle_recover; rclcpp::Service::SharedPtr handle_set_mode_position; rclcpp::Service::SharedPtr handle_set_mode_torque; rclcpp::Service::SharedPtr handle_set_mode_velocity; rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity; rclcpp::Service::SharedPtr handle_set_mode_cyclic_position; rclcpp::Service::SharedPtr handle_set_mode_cyclic_torque; rclcpp::Service::SharedPtr handle_set_mode_interpolated_position; rclcpp::Service::SharedPtr handle_set_target; }; std::vector channels_; uint8_t num_channels_; std::vector channel_names_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publish_joint_state; // Global scale/offset (used as defaults) double scale_pos_to_dev_; double scale_pos_from_dev_; double scale_vel_to_dev_; double scale_vel_from_dev_; double scale_eff_from_dev_; double offset_pos_to_dev_; double offset_pos_from_dev_; ros2_canopen::State402::InternalState switching_state_; int homing_timeout_seconds_; void publish(); virtual void poll_timer_callback() override; void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override; // Shared configure logic for both rclcpp::Node and rclcpp_lifecycle::LifecycleNode void configure_common(); // Helper to create per-channel services after configure void create_per_channel_services(); public: NodeCanopen402Driver(NODETYPE * node); virtual void init(bool called_from_base) override; virtual void configure(bool called_from_base) override; virtual void activate(bool called_from_base) override; virtual void deactivate(bool called_from_base) override; virtual void add_to_master() override; virtual double get_effort(uint8_t channel = 0) { if (channel >= channels_.size() || !channels_[channel].motor) return 0.0; return channels_[channel].motor->get_effort() * channels_[channel].scale_eff_from_dev; } virtual double get_speed(uint8_t channel = 0) { if (channel >= channels_.size() || !channels_[channel].motor) return 0.0; return channels_[channel].motor->get_speed() * channels_[channel].scale_vel_from_dev; } virtual double get_position(uint8_t channel = 0) { if (channel >= channels_.size() || !channels_[channel].motor) return 0.0; return channels_[channel].motor->get_position() * channels_[channel].scale_pos_from_dev + channels_[channel].offset_pos_from_dev; } virtual uint16_t get_mode(uint8_t channel = 0) { if (channel >= channels_.size() || !channels_[channel].motor) return 0; return channels_[channel].motor->getMode(); } /** * @brief Service Callback to initialise device * * Calls Motor402::handleInit function. Brings motor to enabled * state and homes it. * * @param [in] request * @param [out] response */ void handle_init( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to enable device * * Calls Motor402::handleEnable function. Brings motor to enabled * state. * * @param [in] request * @param [out] response */ void handle_enable( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to disable device * * Calls Motor402::handleDisable function. Brings motor to switched on * disabled state. * * @param [in] request * @param [out] response */ void handle_disable( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Method to initialise device * * Calls Motor402::handleInit function. Brings motor to enabled * state and homes it. * * @param [in] void * * @return bool * Indicates initialisation procedure result */ bool init_motor(uint8_t channel = 0); /** * @brief Service Callback to recover device * * Calls Motor402::handleRecover function. Resets faults and brings * motor to enabled state. * * @param [in] request * @param [out] response */ void handle_recover( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Method to recover device * * Calls Motor402::handleRecover function. Resets faults and brings * motor to enabled state. * * @param [in] void * * @return bool */ bool recover_motor(uint8_t channel = 0); /** * @brief Service Callback to halt device * * Calls Motor402::handleHalt function. Calls Quickstop. Resulting * Motor state depends on devices configuration specifically object * 0x605A. * * @param [in] request * @param [out] response */ void handle_halt( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Method to halt device * * Calls Motor402::handleHalt function. Calls Quickstop. Resulting * Motor state depends on devices configuration specifically object * 0x605A. * * @param [in] void * * @return bool */ bool halt_motor(uint8_t channel = 0); /** * @brief Service Callback to set operation mode * * Calls Motor402::enterModeAndWait with requested Operation Mode. * * @param [in] request Requested Operation Mode as MotorBase::Profiled_Position or * MotorBase::Profiled_Velocity or MotorBase::Profiled_Torque or MotorBase::Cyclic_Position or * MotorBase::Cyclic_Velocity or MotorBase::Cyclic_Torque or MotorBase::Interpolated_Position * @param [out] response */ bool set_operation_mode(uint16_t mode, uint8_t channel = 0); /** * @brief Service Callback to set profiled position mode * * Calls Motor402::enterModeAndWait with Profiled Position Mode as * Target Operation Mode. If successful, the motor was transitioned * to Profiled Position Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set profiled velocity mode * * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as * Target Operation Mode. If successful, the motor was transitioned * to Profiled Velocity Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_velocity( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set cyclic position mode * * Calls Motor402::enterModeAndWait with Cyclic Position Mode as * Target Operation Mode. If successful, the motor was transitioned * to Cyclic Position Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_cyclic_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set interpolated position mode * * Calls Motor402::enterModeAndWait with Interpolated Position Mode as * Target Operation Mode. If successful, the motor was transitioned * to Interpolated Position Mode. This only supports linear mode. * * @param [in] request * @param [out] response */ void handle_set_mode_interpolated_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set cyclic velocity mode * * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as * Target Operation Mode. If successful, the motor was transitioned * to Cyclic Velocity Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_cyclic_velocity( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set profiled torque mode * * Calls Motor402::enterModeAndWait with Profiled Torque Mode as * Target Operation Mode. If successful, the motor was transitioned * to Profiled Torque Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_torque( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set cyclic torque mode * * Calls Motor402::enterModeAndWait with Cyclic Torque Mode as * Target Operation Mode. If successful, the motor was transitioned * to Cyclic Torque Mode. * * @param [in] request * @param [out] response */ void handle_set_mode_cyclic_torque( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, uint8_t channel = 0); /** * @brief Service Callback to set target * * Calls Motor402::setTarget and sets the requested target value. Note * that the resulting movement is dependent on the Operation Mode and the * drives state. * * @param [in] request * @param [out] response */ void handle_set_target( const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response, const uint8_t channel = 0); /** * @brief Method to set target * * Calls Motor402::setTarget and sets the requested target value. Note * that the resulting movement is dependent on the Operation Mode and the * drives state. * * @param [in] double target value * @param [in] uint8_t channel (default 0 for backward compatibility) * * @return bool */ bool set_target(double target, uint8_t channel = 0); }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Vishnuprasad Prachandabhanu // Lovro Ivanov // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_ #define NODE_CANOPEN_402_DRIVER_IMPL_HPP_ #include #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "canopen_core/driver_error.hpp" #include using namespace ros2_canopen::node_interfaces; using namespace std::placeholders; template NodeCanopen402Driver::NodeCanopen402Driver(NODETYPE * node) : ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node) { } template void NodeCanopen402Driver::init(bool called_from_base) { RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented."); } template <> void NodeCanopen402Driver::init(bool called_from_base) { NodeCanopenProxyDriver::init(false); publish_joint_state = this->node_->create_publisher("~/joint_states", 1); // Per-channel services will be created in configure() after num_channels_ is known } template <> void NodeCanopen402Driver::init(bool called_from_base) { NodeCanopenProxyDriver::init(false); publish_joint_state = this->node_->create_publisher("~/joint_states", 10); // Per-channel services will be created in configure() after num_channels_ is known } // Helper method to create per-channel services (called from configure) template void NodeCanopen402Driver::create_per_channel_services() { // Ensure per-channel storage exists channels_.resize(num_channels_); // Create services for each channel for (uint8_t ch = 0; ch < num_channels_; ++ch) { // Service naming: // - Single-channel (num_channels_ == 1): Use legacy names like "init", "target", etc. // - Multi-channel: Use channel_name as prefix like "channel_name/init" std::string service_prefix; if (num_channels_ == 1) { // Single-channel: legacy behavior service_prefix = std::string(this->node_->get_name()) + "/"; } else { // Multi-channel: use channel_name as prefix if (ch < channel_names_.size()) { service_prefix = channel_names_[ch] + "/"; } else { // Fallback if channel_names not configured service_prefix = "ch" + std::to_string(ch) + "/"; } } // Init service channels_[ch].handle_init = this->node_->template create_service( service_prefix + "init", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_init(request, response, ch); }); // Enable service channels_[ch].handle_enable = this->node_->template create_service( service_prefix + "enable", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_enable(request, response, ch); }); // Disable service channels_[ch].handle_disable = this->node_->template create_service( service_prefix + "disable", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_disable(request, response, ch); }); // Halt service channels_[ch].handle_halt = this->node_->template create_service( service_prefix + "halt", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_halt(request, response, ch); }); // Recover service channels_[ch].handle_recover = this->node_->template create_service( service_prefix + "recover", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_recover(request, response, ch); }); // Position mode service channels_[ch].handle_set_mode_position = this->node_->template create_service( service_prefix + "position_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_position(request, response, ch); }); // Velocity mode service channels_[ch].handle_set_mode_velocity = this->node_->template create_service( service_prefix + "velocity_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_velocity(request, response, ch); }); // Torque mode service channels_[ch].handle_set_mode_torque = this->node_->template create_service( service_prefix + "torque_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_torque(request, response, ch); }); // Cyclic velocity mode service channels_[ch].handle_set_mode_cyclic_velocity = this->node_->template create_service( service_prefix + "cyclic_velocity_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_cyclic_velocity(request, response, ch); }); // Cyclic position mode service channels_[ch].handle_set_mode_cyclic_position = this->node_->template create_service( service_prefix + "cyclic_position_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_cyclic_position(request, response, ch); }); // Cyclic torque mode service channels_[ch].handle_set_mode_cyclic_torque = this->node_->template create_service( service_prefix + "cyclic_torque_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_cyclic_torque(request, response, ch); }); // Interpolated position mode service channels_[ch].handle_set_mode_interpolated_position = this->node_->template create_service( service_prefix + "interpolated_position_mode", [this, ch]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { this->handle_set_mode_interpolated_position(request, response, ch); }); // Target service (uses COTargetDouble without channel field, since channel is in service name) channels_[ch].handle_set_target = this->node_->template create_service( service_prefix + "target", [this, ch]( const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response) { response->success = this->set_target(request->target, ch); }); } } template void NodeCanopen402Driver::configure_common() { NodeCanopenProxyDriver::configure(false); std::optional scale_pos_to_dev; std::optional scale_pos_from_dev; std::optional scale_vel_to_dev; std::optional scale_vel_from_dev; std::optional scale_eff_from_dev; std::optional offset_pos_to_dev; std::optional offset_pos_from_dev; std::optional switching_state; std::optional homing_timeout_seconds; try { scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].template as()); } catch (...) { } try { scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].template as()); } catch (...) { } try { scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].template as()); } catch (...) { } try { scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].template as()); } catch (...) { } try { scale_eff_from_dev = std::optional(this->config_["scale_eff_from_dev"].template as()); } catch (...) { } try { offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].template as()); } catch (...) { } try { offset_pos_from_dev = std::optional(this->config_["offset_pos_from_dev"].template as()); } catch (...) { } try { switching_state = std::optional(this->config_["switching_state"].template as()); } catch (...) { } try { homing_timeout_seconds = std::optional(this->config_["homing_timeout_seconds"].template as()); } catch (...) { } scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0); scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001); scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0); scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001); scale_eff_from_dev_ = scale_eff_from_dev.value_or(0.001); offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0); offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0); switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or( (int)ros2_canopen::State402::InternalState::Operation_Enable); homing_timeout_seconds_ = homing_timeout_seconds.value_or(10); // Multi-channel configuration num_channels_ = 1; try { num_channels_ = this->config_["num_channels"].template as(); } catch (...) { } // Parse channel names channel_names_.clear(); try { auto names_node = this->config_["channel_names"]; if (names_node.IsDefined() && names_node.IsSequence()) { for (size_t i = 0; i < names_node.size(); ++i) { channel_names_.push_back(names_node[i].template as()); } } } catch (...) { } // Generate default names if not provided if (channel_names_.empty()) { for (uint8_t i = 0; i < num_channels_; ++i) { channel_names_.push_back(std::string(this->node_->get_name()) + "/" + std::to_string(i)); } } // Resolve per-channel scales/offsets into per-channel contexts channels_.clear(); channels_.resize(num_channels_); for (uint8_t i = 0; i < num_channels_; ++i) { channels_[i].scale_pos_to_dev = scale_pos_to_dev_; channels_[i].scale_pos_from_dev = scale_pos_from_dev_; channels_[i].scale_vel_to_dev = scale_vel_to_dev_; channels_[i].scale_vel_from_dev = scale_vel_from_dev_; channels_[i].scale_eff_from_dev = scale_eff_from_dev_; channels_[i].offset_pos_to_dev = offset_pos_to_dev_; channels_[i].offset_pos_from_dev = offset_pos_from_dev_; } try { auto channels_node = this->config_["channels"]; if (channels_node.IsDefined() && channels_node.IsSequence()) { for (size_t i = 0; i < channels_node.size() && i < num_channels_; ++i) { auto ch = channels_node[i]; try { channels_[i].scale_pos_to_dev = ch["scale_pos_to_dev"].template as(); } catch (...) { } try { channels_[i].scale_pos_from_dev = ch["scale_pos_from_dev"].template as(); } catch (...) { } try { channels_[i].scale_vel_to_dev = ch["scale_vel_to_dev"].template as(); } catch (...) { } try { channels_[i].scale_vel_from_dev = ch["scale_vel_from_dev"].template as(); } catch (...) { } try { channels_[i].scale_eff_from_dev = ch["scale_eff_from_dev"].template as(); } catch (...) { } try { channels_[i].offset_pos_to_dev = ch["offset_pos_to_dev"].template as(); } catch (...) { } try { channels_[i].offset_pos_from_dev = ch["offset_pos_from_dev"].template as(); } catch (...) { } } } } catch (...) { } RCLCPP_INFO( this->node_->get_logger(), "num_channels: %u\nscale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ " "%f\nscale_vel_from_dev_ " "%f\nscale_eff_from_dev_ %f\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ " "%f\nhoming_timeout_seconds_ %i\n", num_channels_, scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_, scale_eff_from_dev_, offset_pos_to_dev_, offset_pos_from_dev_, homing_timeout_seconds_); // Create per-channel services create_per_channel_services(); } template <> void NodeCanopen402Driver::configure(bool called_from_base) { this->configure_common(); } template <> void NodeCanopen402Driver::configure(bool called_from_base) { this->configure_common(); } template void NodeCanopen402Driver::activate(bool called_from_base) { NodeCanopenProxyDriver::activate(false); // Register default modes for all motor instances for (auto & ch : channels_) { if (!ch.motor) continue; ch.motor->registerDefaultModes(); ch.motor->set_diagnostic_status_msgs(this->diagnostic_collector_, this->diagnostic_enabled_); } } template void NodeCanopen402Driver::deactivate(bool called_from_base) { NodeCanopenProxyDriver::deactivate(false); timer_->cancel(); } template void NodeCanopen402Driver::poll_timer_callback() { NodeCanopenProxyDriver::poll_timer_callback(); // Handle read/write for all motors for (auto & ch : channels_) { if (!ch.motor) continue; ch.motor->handleRead(); ch.motor->handleWrite(); } publish(); } template void NodeCanopen402Driver::publish() { sensor_msgs::msg::JointState js_msg; // Publish joint state for all channels for (size_t ch = 0; ch < channels_.size(); ++ch) { std::string name; if (num_channels_ == 1) { // Single-channel: use node name (legacy behavior) name = std::string(this->node_->get_name()); } else { // Multi-channel: use channel_name if (ch < channel_names_.size()) { name = channel_names_[ch]; } else { // Fallback if channel_names not configured name = std::string(this->node_->get_name()) + "/ch" + std::to_string(ch); } } js_msg.name.push_back(name); if (!channels_[ch].motor) { js_msg.position.push_back(0.0); js_msg.velocity.push_back(0.0); js_msg.effort.push_back(0.0); continue; } js_msg.position.push_back( channels_[ch].motor->get_position() * channels_[ch].scale_pos_from_dev + channels_[ch].offset_pos_from_dev); js_msg.velocity.push_back(channels_[ch].motor->get_speed() * channels_[ch].scale_vel_from_dev); js_msg.effort.push_back(channels_[ch].motor->get_effort() * channels_[ch].scale_eff_from_dev); } publish_joint_state->publish(js_msg); } template void NodeCanopen402Driver::add_to_master() { NodeCanopenProxyDriver::add_to_master(); // Create separate Motor402 instance for each channel channels_.resize(num_channels_); for (uint8_t ch = 0; ch < num_channels_; ++ch) { channels_[ch].motor = std::make_shared(this->lely_driver_, switching_state_, homing_timeout_seconds_, ch); } } template void NodeCanopen402Driver::handle_init( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = this->init_motor(channel); } template void NodeCanopen402Driver::handle_recover( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = this->recover_motor(channel); } template void NodeCanopen402Driver::handle_halt( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = this->halt_motor(channel); } template void NodeCanopen402Driver::handle_set_mode_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Profiled_Position, channel); } template void NodeCanopen402Driver::handle_set_mode_velocity( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Profiled_Velocity, channel); } template void NodeCanopen402Driver::handle_set_mode_cyclic_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Position, channel); } template void NodeCanopen402Driver::handle_set_mode_interpolated_position( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Interpolated_Position, channel); } template void NodeCanopen402Driver::handle_set_mode_cyclic_velocity( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Velocity, channel); } template void NodeCanopen402Driver::handle_set_mode_torque( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Profiled_Torque, channel); } template void NodeCanopen402Driver::handle_set_mode_cyclic_torque( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { response->success = set_operation_mode(MotorBase::Cyclic_Synchronous_Torque, channel); } template void NodeCanopen402Driver::handle_set_target( const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response, const uint8_t channel) { response->success = set_target(request->target, channel); } template void NodeCanopen402Driver::handle_disable( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size() || !channels_[channel].motor) { response->success = false; return; } response->success = channels_[channel].motor->handleDisable(); } else { response->success = false; } } template void NodeCanopen402Driver::handle_enable( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response, const uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size() || !channels_[channel].motor) { response->success = false; return; } response->success = channels_[channel].motor->handleEnable(); } else { response->success = false; } } template bool NodeCanopen402Driver::init_motor(uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size()) { RCLCPP_ERROR( this->node_->get_logger(), "Invalid channel %u (max %lu)", channel, channels_.size() - 1); return false; } if (!channels_[channel].motor) return false; return channels_[channel].motor->handleInit(); } else { RCLCPP_INFO(this->node_->get_logger(), "Initialisation failed."); return false; } } template bool NodeCanopen402Driver::recover_motor(uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size()) { RCLCPP_ERROR( this->node_->get_logger(), "Invalid channel %u (max %lu)", channel, channels_.size() - 1); return false; } if (!channels_[channel].motor) return false; return channels_[channel].motor->handleRecover(); } else { return false; } } template bool NodeCanopen402Driver::halt_motor(uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size()) { RCLCPP_ERROR( this->node_->get_logger(), "Invalid channel %u (max %lu)", channel, channels_.size() - 1); return false; } if (!channels_[channel].motor) return false; return channels_[channel].motor->handleHalt(); } else { return false; } } template bool NodeCanopen402Driver::set_operation_mode(uint16_t mode, uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size()) { RCLCPP_ERROR( this->node_->get_logger(), "Invalid channel %u (max %lu)", channel, channels_.size() - 1); return false; } if (!channels_[channel].motor) return false; if (channels_[channel].motor->getMode() != mode) { return channels_[channel].motor->enterModeAndWait(mode); } else { return false; } } return false; } template bool NodeCanopen402Driver::set_target(double target, uint8_t channel) { if (this->activated_.load()) { if (channel >= channels_.size()) { RCLCPP_ERROR( this->node_->get_logger(), "Invalid channel %u (max %lu)", channel, channels_.size() - 1); return false; } if (!channels_[channel].motor) return false; auto mode = channels_[channel].motor->getMode(); const double scale_pos_to_dev = channels_[channel].scale_pos_to_dev; const double offset_pos_to_dev = channels_[channel].offset_pos_to_dev; const double scale_vel_to_dev = channels_[channel].scale_vel_to_dev; double scaled_target; if ( (mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or (mode == MotorBase::Interpolated_Position)) { scaled_target = target * scale_pos_to_dev + offset_pos_to_dev; } else if ( (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or (mode == MotorBase::Cyclic_Synchronous_Velocity)) { scaled_target = target * scale_vel_to_dev; } else { scaled_target = target; } return channels_[channel].motor->setTarget(scaled_target); } else { return false; } } template void NodeCanopen402Driver::diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { // Handle diagnostics for all motors for (size_t ch = 0; ch < channels_.size(); ++ch) { if (!channels_[ch].motor) continue; channels_[ch].motor->handleDiag(); } stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage()); stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE")); stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT")); stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY")); stat.add("cia402_mode", this->diagnostic_collector_->getValue("cia402_mode")); stat.add("cia402_state", this->diagnostic_collector_->getValue("cia402_state")); // Add per-channel diagnostic info for (size_t ch = 0; ch < channels_.size(); ++ch) { std::string ch_prefix = "ch" + std::to_string(ch) + "_"; stat.add( ch_prefix + "mode", this->diagnostic_collector_->getValue((ch_prefix + "cia402_mode").c_str())); stat.add( ch_prefix + "state", this->diagnostic_collector_->getValue((ch_prefix + "cia402_state").c_str())); } } #endif ================================================ FILE: canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef PROFILED_POSITION_MODE_HPP #define PROFILED_POSITION_MODE_HPP #include #include #include "canopen_base_driver/lely_driver_bridge.hpp" #include "mode_target_helper.hpp" namespace ros2_canopen { class ProfiledPositionMode : public ModeTargetHelper { const uint16_t base_index = 0x607A; uint16_t channel_offset_; // Channel offset for multi-axis support (CiA 402-2) std::shared_ptr driver; double last_target_; uint16_t sw_; public: enum SW_masks { MASK_Reached = (1 << State402::SW_Target_reached), MASK_Acknowledged = (1 << State402::SW_Operation_mode_specific0), MASK_Error = (1 << State402::SW_Operation_mode_specific1), }; enum CW_bits { CW_NewPoint = Command402::CW_Operation_mode_specific0, CW_Immediate = Command402::CW_Operation_mode_specific1, CW_Blending = Command402::CW_Operation_mode_specific3, }; ProfiledPositionMode(std::shared_ptr driver, uint16_t channel_offset = 0) : ModeTargetHelper(MotorBase::Profiled_Position), channel_offset_(channel_offset) { this->driver = driver; } virtual bool start() { sw_ = 0; last_target_ = std::numeric_limits::quiet_NaN(); return ModeTargetHelper::start(); } virtual bool read(const uint16_t & sw) { sw_ = sw; return (sw & MASK_Error) == 0; } virtual bool write(OpModeAccesser & cw) { cw.set(CW_Immediate); if (hasTarget()) { int32_t target = getTarget(); if ((sw_ & MASK_Acknowledged) == 0 && target != last_target_) { if (cw.get(CW_NewPoint)) { cw.reset(CW_NewPoint); // reset if needed } else { // Apply channel offset to object dictionary index driver->universal_set_value(base_index + channel_offset_, 0x0, target); cw.set(CW_NewPoint); last_target_ = target; } } else if (sw_ & MASK_Acknowledged) { cw.reset(CW_NewPoint); } return true; } return false; } }; } // namespace ros2_canopen #endif // PROFILED_POSITION_MODE_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/state.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER_STATE_HPP #define CANOPEN_402_DRIVER_STATE_HPP #include #include #include namespace ros2_canopen { class State402 { public: enum StatusWord { SW_Ready_To_Switch_On = 0, SW_Switched_On = 1, SW_Operation_enabled = 2, SW_Fault = 3, SW_Voltage_enabled = 4, SW_Quick_stop = 5, SW_Switch_on_disabled = 6, SW_Warning = 7, SW_Manufacturer_specific0 = 8, SW_Remote = 9, SW_Target_reached = 10, SW_Internal_limit = 11, SW_Operation_mode_specific0 = 12, SW_Operation_mode_specific1 = 13, SW_Manufacturer_specific1 = 14, SW_Manufacturer_specific2 = 15 }; enum InternalState { Unknown = 0, Start = 0, Not_Ready_To_Switch_On = 1, Switch_On_Disabled = 2, Ready_To_Switch_On = 3, Switched_On = 4, Operation_Enable = 5, Quick_Stop_Active = 6, Fault_Reaction_Active = 7, Fault = 8, }; InternalState getState(); InternalState read(uint16_t sw); bool waitForNewState( const std::chrono::steady_clock::time_point & abstime, InternalState & state); State402() : state_(Unknown) {} private: std::condition_variable cond_; std::mutex mutex_; InternalState state_; }; } // namespace ros2_canopen #endif // CANOPEN_402_DRIVER_STATE_HPP ================================================ FILE: canopen_402_driver/include/canopen_402_driver/visibility_control.h ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ #define CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_402_DRIVER_EXPORT __attribute__((dllexport)) #define CANOPEN_402_DRIVER_IMPORT __attribute__((dllimport)) #else #define CANOPEN_402_DRIVER_EXPORT __declspec(dllexport) #define CANOPEN_402_DRIVER_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_402_DRIVER_BUILDING_LIBRARY #define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_EXPORT #else #define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_IMPORT #endif #define CANOPEN_402_DRIVER_PUBLIC_TYPE CANOPEN_402_DRIVER_PUBLIC #define CANOPEN_402_DRIVER_LOCAL #else #define CANOPEN_402_DRIVER_EXPORT __attribute__((visibility("default"))) #define CANOPEN_402_DRIVER_IMPORT #if __GNUC__ >= 4 #define CANOPEN_402_DRIVER_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_402_DRIVER_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_402_DRIVER_PUBLIC #define CANOPEN_402_DRIVER_LOCAL #endif #define CANOPEN_402_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_402_driver/include/canopen_402_driver/word_accessor.hpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #ifndef WORD_ACCESSOR_HPP #define WORD_ACCESSOR_HPP #include namespace ros2_canopen { template class WordAccessor { uint16_t & word_; public: WordAccessor(uint16_t & word) : word_(word) {} bool set(uint8_t bit) { uint16_t val = MASK & (1 << bit); word_ |= val; return val; } bool reset(uint8_t bit) { uint16_t val = MASK & (1 << bit); word_ &= ~val; return val; } bool get(uint8_t bit) const { return word_ & (1 << bit); } uint16_t get() const { return word_ & MASK; } WordAccessor & operator=(const uint16_t & val) { word_ = (word_ & ~MASK) | (val & MASK); return *this; } }; } // namespace ros2_canopen #endif // WORD_ACCESSOR_HPP ================================================ FILE: canopen_402_driver/package.xml ================================================ canopen_402_driver 0.3.2 Driver for devices implementing CIA402 profile Christoph Hellmann Santos LGPL-v3 ament_cmake_ros boost canopen_base_driver canopen_core canopen_interfaces canopen_proxy_driver rclcpp rclcpp_components rclcpp_lifecycle sensor_msgs ament_lint_auto ament_cmake ================================================ FILE: canopen_402_driver/src/cia402_driver.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/cia402_driver.hpp" using namespace ros2_canopen; Cia402Driver::Cia402Driver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) { node_canopen_402_driver_ = std::make_shared>(this); node_canopen_driver_ = std::static_pointer_cast(node_canopen_402_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::Cia402Driver) ================================================ FILE: canopen_402_driver/src/command.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/command.hpp" #include using namespace ros2_canopen; const Command402::TransitionTable Command402::transitions_; Command402::TransitionTable::TransitionTable() { typedef State402 s; transitions_.reserve(32); Op disable_voltage(0, (1 << CW_Fault_Reset) | (1 << CW_Enable_Voltage)); /* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage); /* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage); /*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage); /*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage); Op automatic(0, 0); /* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic); /* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic); /*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic); Op shutdown( (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Switch_On)); /* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown); /* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown); /* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown); Op switch_on( (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On), (1 << CW_Fault_Reset) | (1 << CW_Enable_Operation)); /* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on); /* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on); Op enable_operation( (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On) | (1 << CW_Enable_Operation), (1 << CW_Fault_Reset)); /* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation); /*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation); Op quickstop((1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Quick_Stop)); /* 7*/ add( s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled /*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled /*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop); // fault reset /*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1 << CW_Fault_Reset), 0)); } State402::InternalState Command402::nextStateForEnabling(State402::InternalState state) { switch (state) { case State402::Start: return State402::Not_Ready_To_Switch_On; case State402::Fault: case State402::Not_Ready_To_Switch_On: return State402::Switch_On_Disabled; case State402::Switch_On_Disabled: return State402::Ready_To_Switch_On; case State402::Ready_To_Switch_On: return State402::Switched_On; case State402::Switched_On: case State402::Quick_Stop_Active: case State402::Operation_Enable: return State402::Operation_Enable; case State402::Fault_Reaction_Active: return State402::Fault; } throw std::out_of_range("state value is illegal"); } bool Command402::setTransition( uint16_t & cw, const State402::InternalState & from, const State402::InternalState & to, State402::InternalState * next) { try { if (from != to) { State402::InternalState hop = to; if (next) { if (to == State402::Operation_Enable) hop = nextStateForEnabling(from); *next = hop; } transitions_.get(from, hop)(cw); } return true; } catch (...) { /// @todo Print error here. } return false; } ================================================ FILE: canopen_402_driver/src/default_homing_mode.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/default_homing_mode.hpp" using namespace ros2_canopen; template struct masked_status_not_equal { uint16_t & status_; masked_status_not_equal(uint16_t & status) : status_(status) {} bool operator()() const { return (status_ & mask) != not_equal; } }; bool DefaultHomingMode::start() { execute_ = false; return read(0); } bool DefaultHomingMode::read(const uint16_t & sw) { std::scoped_lock lock(mutex_); uint16_t old = status_; status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error); if (old != status_) { cond_.notify_all(); } return true; } bool DefaultHomingMode::write(Mode::OpModeAccesser & cw) { cw = 0; if (execute_) { cw.set(CW_StartHoming); return true; } return false; } bool DefaultHomingMode::executeHoming() { // Apply channel offset to object dictionary index int hmode = driver->universal_get_value(base_index + channel_offset_, 0x0); if (hmode == 0) { return true; } /// @ get abs time from canopen_master std::chrono::steady_clock::time_point prepare_time = std::chrono::steady_clock::now() + std::chrono::seconds(1); // ensure homing is not running std::unique_lock lock(mutex_); if (!cond_.wait_until( lock, prepare_time, masked_status_not_equal(status_))) { return error("could not prepare homing"); } if (status_ & MASK_Error) { return error("homing error before start"); } execute_ = true; // ensure start if (!cond_.wait_until( lock, prepare_time, masked_status_not_equal(status_))) { return error("homing did not start"); } if (status_ & MASK_Error) { return error("homing error at start"); } // std::chrono::steady_clock::time_point finish_time = // std::chrono::steady_clock::now() + std::chrono::seconds(10); // // std::chrono::steady_clock::time_point finish_time = std::chrono::steady_clock::now() + std::chrono::seconds(homing_timeout_seconds_); // // wait for attained if (!cond_.wait_until( lock, finish_time, masked_status_not_equal(status_))) { return error("homing not attained"); } if (status_ & MASK_Error) { return error("homing error during process"); } // wait for motion stop if (!cond_.wait_until( lock, finish_time, masked_status_not_equal(status_))) { return error("homing did not stop"); } if (status_ & MASK_Error) { return error("homing error during stop"); } if ((status_ & MASK_Reached) && (status_ & MASK_Attained)) { execute_ = false; return true; } return error("something went wrong while homing"); } ================================================ FILE: canopen_402_driver/src/lifecycle_cia402_driver.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/lifecycle_cia402_driver.hpp" using namespace ros2_canopen; LifecycleCia402Driver::LifecycleCia402Driver(rclcpp::NodeOptions node_options) : LifecycleCanopenDriver(node_options) { node_canopen_402_driver_ = std::make_shared>(this); node_canopen_driver_ = std::static_pointer_cast(node_canopen_402_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleCia402Driver) ================================================ FILE: canopen_402_driver/src/motor.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2023 Vishnuprasad Prachandabhanu // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/motor.hpp" using namespace ros2_canopen; bool Motor402::setTarget(double val) { if (state_handler_.getState() == State402::Operation_Enable) { std::scoped_lock lock(mode_mutex_); return selected_mode_ && selected_mode_->setTarget(val); } return false; } bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); } bool Motor402::enterModeAndWait(uint16_t mode) { bool okay = mode != MotorBase::Homing && switchMode(mode); return okay; } uint16_t Motor402::getMode() { std::scoped_lock lock(mode_mutex_); return selected_mode_ ? selected_mode_->mode_id_ : (uint16_t)MotorBase::No_Mode; } bool Motor402::isModeSupportedByDevice(uint16_t mode) { uint32_t supported_modes = driver->universal_get_value(get_channel_index(supported_drive_modes_index), 0x0); bool supported = supported_modes & (1 << (mode - 1)); bool below_max = mode <= 32; bool above_min = mode > 0; return below_max && above_min && supported; } void Motor402::registerMode(uint16_t id, const ModeSharedPtr & m) { std::scoped_lock map_lock(map_mutex_); if (m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m)); } ModeSharedPtr Motor402::allocMode(uint16_t mode) { ModeSharedPtr res; if (isModeSupportedByDevice(mode)) { std::scoped_lock map_lock(map_mutex_); std::unordered_map::iterator it = modes_.find(mode); if (it != modes_.end()) { res = it->second; } } return res; } bool Motor402::switchMode(uint16_t mode) { if (mode == MotorBase::No_Mode) { std::scoped_lock lock(mode_mutex_); selected_mode_.reset(); try { // try to set mode (use channel-specific index) driver->universal_set_value(get_channel_index(op_mode_index), 0x0, mode); } catch (...) { } if (enable_diagnostics_.load()) { this->diag_collector_->addf( "cia402_mode", "No mode selected: %d (channel %u)", mode, channel_); } return true; } ModeSharedPtr next_mode = allocMode(mode); if (!next_mode) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode is not supported."); return false; } if (!next_mode->start()) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not start mode."); return false; } { // disable mode handler std::scoped_lock lock(mode_mutex_); if (mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode) { // nothing to do return true; } selected_mode_.reset(); } if (!switchState(switching_state_)) return false; driver->universal_set_value(get_channel_index(op_mode_index), 0x0, mode); bool okay = false; { // wait for switch std::unique_lock lock(mode_mutex_); std::chrono::steady_clock::time_point abstime = std::chrono::steady_clock::now() + std::chrono::seconds(5); if (monitor_mode_) { while (mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) { } } else { while (mode_id_ != mode && std::chrono::steady_clock::now() < abstime) { lock.unlock(); // unlock inside loop driver->universal_get_value(get_channel_index(op_mode_display_index), 0x0); // poll std::this_thread::sleep_for(std::chrono::milliseconds(20)); // wait some time lock.lock(); } } if (mode_id_ == mode) { selected_mode_ = next_mode; okay = true; if (enable_diagnostics_.load()) { this->diag_collector_->addf( "cia402_mode", "Mode switched to: %d (channel %u)", mode, channel_); } } else { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode switch timed out."); driver->universal_set_value(get_channel_index(op_mode_index), 0x0, mode_id_); if (enable_diagnostics_.load()) { this->diag_collector_->addf( "cia402_mode", "Mode switch timed out: %d (channel %u)", mode, channel_); } } } if (!switchState(State402::Operation_Enable)) return false; return okay; } bool Motor402::switchState(const State402::InternalState & target) { std::chrono::steady_clock::time_point abstime = std::chrono::steady_clock::now() + state_switch_timeout_; State402::InternalState state = state_handler_.getState(); target_state_ = target; while (state != target_state_) { std::unique_lock lock(cw_mutex_); State402::InternalState next = State402::Unknown; bool success = Command402::setTransition(control_word_, state, target_state_, &next); if (!success) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not set transition."); return false; } else if (enable_diagnostics_.load() && success) { this->diag_collector_->addf("cia402_state", "State switched to: %d", next); } lock.unlock(); if (state != next && !state_handler_.waitForNewState(abstime, state)) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Transition timed out."); if (enable_diagnostics_.load()) { this->diag_collector_->addf( "cia402_state", "State transition timed out: %d -> %d", state, next); } return false; } } return state == target; } bool Motor402::readState() { uint16_t old_sw, sw = driver->universal_get_value( get_channel_index(status_word_entry_index), 0x0); // TODO: added error handling old_sw = status_word_.exchange(sw); state_handler_.read(sw); std::unique_lock lock(mode_mutex_); uint16_t new_mode; new_mode = driver->universal_get_value(get_channel_index(op_mode_display_index), 0x0); // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode %hhi",new_mode); if (selected_mode_ && selected_mode_->mode_id_ == new_mode) { if (!selected_mode_->read(sw)) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode handler has error."); } } if (new_mode != mode_id_) { mode_id_ = new_mode; mode_cond_.notify_all(); } if (selected_mode_ && selected_mode_->mode_id_ != new_mode) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode does not match."); } if (sw & (1 << State402::SW_Internal_limit)) { if (old_sw & (1 << State402::SW_Internal_limit)) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); } else { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); } } return true; } void Motor402::handleRead() { readState(); } void Motor402::handleWrite() { std::scoped_lock lock(cw_mutex_); control_word_ |= (1 << Command402::CW_Halt); if (state_handler_.getState() == State402::Operation_Enable) { std::scoped_lock lock(mode_mutex_); Mode::OpModeAccesser cwa(control_word_); bool okay = false; if (selected_mode_ && selected_mode_->mode_id_ == mode_id_) { okay = selected_mode_->write(cwa); } else { cwa = 0; } if (okay) { control_word_ &= ~(1 << Command402::CW_Halt); } } if (start_fault_reset_.exchange(false)) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Fault reset (channel %u)", channel_); this->driver->universal_set_value( get_channel_index(control_word_entry_index), 0x0, control_word_ & ~(1 << Command402::CW_Fault_Reset)); } else { // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Control Word %s", // std::bitset<16>{control_word_}.to_string()); this->driver->universal_set_value( get_channel_index(control_word_entry_index), 0x0, control_word_); } } void Motor402::handleDiag() { uint16_t sw = status_word_; State402::InternalState state = state_handler_.getState(); switch (state) { case State402::Not_Ready_To_Switch_On: this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "Not ready to switch on"); break; case State402::Switch_On_Disabled: this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "Switch on disabled"); break; case State402::Ready_To_Switch_On: this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::OK, "Ready to switch on"); break; case State402::Switched_On: // std::cout << "Motor operation is not enabled" << std::endl; this->diag_collector_->summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Switched on"); break; case State402::Operation_Enable: this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::OK, "Operation enabled"); break; case State402::Quick_Stop_Active: // std::cout << "Quick stop is active" << std::endl; this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "Quick stop active"); break; case State402::Fault: this->diag_collector_->summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Fault"); break; case State402::Fault_Reaction_Active: // std::cout << "Motor has fault" << std::endl; this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Fault reaction active"); break; case State402::Unknown: // std::cout << "State is unknown" << std::endl; this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown state"); break; } if (sw & (1 << State402::SW_Warning)) { // std::cout << "Warning bit is set" << std::endl; this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "Warning bit is set"); } if (sw & (1 << State402::SW_Internal_limit)) { // std::cout << "Internal limit active" << std::endl; this->diag_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "Internal limit active"); } } bool Motor402::handleInit() { for (std::unordered_map::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it) { (it->second)(); } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Read State"); if (!readState()) { std::cout << "Could not read motor state" << std::endl; return false; } { std::scoped_lock lock(cw_mutex_); control_word_ = 0; start_fault_reset_ = true; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Enable"); if (!switchState(State402::Operation_Enable)) { std::cout << "Could not enable motor" << std::endl; return false; } ModeSharedPtr m = allocMode(MotorBase::Homing); if (!m) { std::cout << "Homeing mode not supported" << std::endl; return true; // homing not supported } HomingMode * homing = dynamic_cast(m.get()); if (!homing) { std::cout << "Homing mode has incorrect handler" << std::endl; return false; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch to homing"); if (!switchMode(MotorBase::Homing)) { std::cout << "Could not enter homing mode" << std::endl; return false; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Execute homing"); if (!homing->executeHoming()) { std::cout << "Homing failed" << std::endl; return false; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch no mode"); if (!switchMode(MotorBase::No_Mode)) { std::cout << "Could not enter no mode" << std::endl; return false; } return true; } bool Motor402::handleShutdown() { switchMode(MotorBase::No_Mode); return switchState(State402::Switch_On_Disabled); } bool Motor402::handleHalt() { State402::InternalState state = state_handler_.getState(); std::scoped_lock lock(cw_mutex_); // do not demand quickstop in case of fault if (state == State402::Fault_Reaction_Active || state == State402::Fault) return false; if (state != State402::Operation_Enable) { target_state_ = state; } else { target_state_ = State402::Quick_Stop_Active; if (!Command402::setTransition(control_word_, state, State402::Quick_Stop_Active, 0)) { std::cout << "Could not quick stop" << std::endl; return false; } } return true; } bool Motor402::handleRecover() { start_fault_reset_ = true; { std::scoped_lock lock(mode_mutex_); if (selected_mode_ && !selected_mode_->start()) { std::cout << "Could not restart mode." << std::endl; return false; } } if (!switchState(State402::Operation_Enable)) { std::cout << "Could not enable motor" << std::endl; return false; } return true; } bool Motor402::handleEnable() { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Enable: Read State"); if (!readState()) { std::cout << "Could not read motor state" << std::endl; return false; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Enable"); if (!switchState(State402::Operation_Enable)) { std::cout << "Could not enable motor" << std::endl; return false; } return true; } bool Motor402::handleDisable() { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Disable: Read State"); if (!readState()) { std::cout << "Could not read motor state" << std::endl; return false; } RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Disable"); if (!switchState(State402::Switched_On)) { std::cout << "Could not disable motor" << std::endl; return false; } return true; } ================================================ FILE: canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp" #include "canopen_core/driver_error.hpp" using namespace ros2_canopen::node_interfaces; template class ros2_canopen::node_interfaces::NodeCanopen402Driver; template class ros2_canopen::node_interfaces::NodeCanopen402Driver; ================================================ FILE: canopen_402_driver/src/state.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // Copyright 2014-2022 Authors of ros_canopen // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include "canopen_402_driver/state.hpp" #include using namespace ros2_canopen; State402::InternalState State402::getState() { std::scoped_lock lock(mutex_); return state_; } State402::InternalState State402::read(uint16_t sw) { static const uint16_t r = (1 << SW_Ready_To_Switch_On); static const uint16_t s = (1 << SW_Switched_On); static const uint16_t o = (1 << SW_Operation_enabled); static const uint16_t f = (1 << SW_Fault); static const uint16_t q = (1 << SW_Quick_stop); static const uint16_t d = (1 << SW_Switch_on_disabled); InternalState new_state = Unknown; uint16_t state = sw & (d | q | f | o | s | r); switch (state) { // ( d | q | f | o | s | r ): case (0 | 0 | 0 | 0 | 0 | 0): case (0 | q | 0 | 0 | 0 | 0): // std::cout << "Not_Ready_To_Switch_On" << std::endl; new_state = Not_Ready_To_Switch_On; break; case (d | 0 | 0 | 0 | 0 | 0): case (d | q | 0 | 0 | 0 | 0): // std::cout << "Switch_On_Disabled" << std::endl; new_state = Switch_On_Disabled; break; case (0 | q | 0 | 0 | 0 | r): // std::cout << "Ready_To_Switch_On" << std::endl; new_state = Ready_To_Switch_On; break; case (0 | q | 0 | 0 | s | r): new_state = Switched_On; break; case (0 | q | 0 | o | s | r): new_state = Operation_Enable; break; case (0 | 0 | 0 | o | s | r): new_state = Quick_Stop_Active; break; case (0 | 0 | f | o | s | r): case (0 | q | f | o | s | r): new_state = Fault_Reaction_Active; break; case (0 | 0 | f | 0 | 0 | 0): case (0 | q | f | 0 | 0 | 0): new_state = Fault; break; default: /// @todo Throw error here. break; } std::scoped_lock lock(mutex_); if (new_state != state_) { state_ = new_state; cond_.notify_all(); } return state_; } bool State402::waitForNewState( const std::chrono::steady_clock::time_point & abstime, State402::InternalState & state) { std::unique_lock lock(mutex_); while (state_ == state && cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) { } bool res = state != state_; state = state_; return res; } ================================================ FILE: canopen_402_driver/test/CMakeLists.txt ================================================ ament_add_gtest(test_driver_component test_driver_component.cpp ) target_include_directories(test_driver_component PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_driver_component ${canopen_interfaces_TARGETS} ${sensor_msgs_TARGETS} canopen_base_driver::base_driver canopen_base_driver::lely_driver_bridge canopen_base_driver::lifecycle_base_driver canopen_base_driver::node_canopen_base_driver canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master canopen_proxy_driver::lifecycle_proxy_driver canopen_proxy_driver::node_canopen_proxy_driver canopen_proxy_driver::proxy_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle sensor_msgs::sensor_msgs_library ) ================================================ FILE: canopen_402_driver/test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=2 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=32 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1A00 20=0x1A01 21=0x1F25 22=0x1F55 23=0x1F80 24=0x1F81 25=0x1F82 26=0x1F84 27=0x1F85 28=0x1F86 29=0x1F87 30=0x1F88 31=0x1F89 32=0x1F8A [ManufacturerObjects] SupportedObjects=12 1=0x2000 2=0x2001 3=0x2200 4=0x2201 5=0x5800 6=0x5801 7=0x5A00 8=0x5A01 9=0x5C00 10=0x5C01 11=0x5E00 12=0x5E01 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=2 2=0x00000082 3=0x00000083 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=2 2=0x00000005 3=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=2 2=0x00000360 3=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=proxy_device_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=proxy_device_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=proxy_device_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=proxy_device_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_402_driver/test/test_driver_component.cpp ================================================ // Copyright 2023 Christoph Hellmann Santos // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . // #include #include #include #include #include #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "gtest/gtest.h" using namespace rclcpp_components; TEST(ComponentLoad, test_load_component_1) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_402_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[0]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } TEST(ComponentLoad, test_load_component_2) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_402_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[1]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } ================================================ FILE: canopen_base_driver/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.2.9 (2024-04-16) ------------------ * Add timeouts * Contributors: Vishnuprasad Prachandabhanu 0.3.2 (2025-12-05) ------------------ * `#379 `_: Fix data conversion in the Lely Bridge to enable more data types and proper handling of Emcy in ros2_control * Fixed types handling in canopen_ros2_control. * `#376 `_: increase boot timout * Contributors: Dr. Denis Stogl, Vishnuprasad Prachandabhanu 0.3.1 (2025-06-23) ------------------ * Add boot timeout and retry * Include driver exception when boot failed * Boot Timeout: Add parameter to base driver to pass to wait as timeout * Contributors: Gerry Salinas, Luis Camero, Vishnuprasad Prachandabhanu, ipa-vsp 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * Merge pull request `#280 `_ from ipa-vsp/fix/yaml-build-error Fix undefined reference to yaml library * pre-commit update * Merge pull request `#261 `_ from gsalinas/configurable-sdo-timeout Configurable SDO timeout * 0.2.9 * forthcoming * Merge pull request `#220 `_ from ipa-vsp/feature/timeout-config Add timeouts * Set SDO timeout from node config. * Replace two more hardcoded timeouts. * Include timeout in documentation comment for LelyDriverBridge. * Make 20ms a default argument of the master & driver bridges. * change error to warn for testing * remove build warings * non transmit time from bus.yml * Contributors: Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Update printed output in lely_driver_bridge.cpp * Contributors: Christoph Hellmann Santos, yos627 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix base driver lifecyle * Fix node polling mode in base driver * Contributors: Błażej Sowa, Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_base_driver/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_base_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(lely_core_libraries REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) add_library(lely_driver_bridge src/lely_driver_bridge.cpp ) target_compile_features(lely_driver_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lely_driver_bridge PUBLIC -Wl,--no-undefined) target_include_directories(lely_driver_bridge PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(lely_driver_bridge PUBLIC ${lely_core_libraries_LIBRARIES} canopen_core::node_canopen_driver rclcpp::rclcpp ) add_library(node_canopen_base_driver src/node_interfaces/node_canopen_base_driver.cpp ) target_compile_features(node_canopen_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_base_driver PUBLIC -Wl,--no-undefined) target_include_directories(node_canopen_base_driver PUBLIC $ $) target_link_libraries(node_canopen_base_driver PUBLIC ${canopen_interfaces_TARGETS} ${std_msgs_TARGETS} ${std_srvs_TARGETS} Boost::thread canopen_core::node_canopen_driver diagnostic_updater::diagnostic_updater lely_driver_bridge ) message(STATUS "node_canopen_base_driver: ${lely_core_libraries_LIBRARIES}") add_library(lifecycle_base_driver src/lifecycle_base_driver.cpp ) target_compile_features(lifecycle_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_base_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_base_driver PUBLIC $ $) target_link_libraries(lifecycle_base_driver PUBLIC canopen_core::node_canopen_driver node_canopen_base_driver rclcpp_components::component rclcpp_lifecycle::rclcpp_lifecycle ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(lifecycle_base_driver PRIVATE "CANOPEN_base_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(lifecycle_base_driver "ros2_canopen::LifecycleBaseDriver") set(node_plugins "${node_plugins}ros2_canopen::LifecycleBaseDriver;$\n") add_library(base_driver src/base_driver.cpp ) target_compile_features(base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(base_driver PUBLIC -Wl,--no-undefined) target_include_directories(base_driver PUBLIC $ $) target_link_libraries(base_driver PUBLIC canopen_core::node_canopen_driver node_canopen_base_driver rclcpp_components::component ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(base_driver PRIVATE "CANOPEN_base_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(base_driver "ros2_canopen::BaseDriver") set(node_plugins "${node_plugins}ros2_canopen::BaseDriver;$\n") install( DIRECTORY include/ DESTINATION include ) install( TARGETS lifecycle_base_driver base_driver node_canopen_base_driver lely_driver_bridge EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( lifecycle_base_driver base_driver node_canopen_base_driver lely_driver_bridge ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( canopen_core canopen_interfaces diagnostic_updater lely_core_libraries rclcpp rclcpp_components rclcpp_lifecycle std_msgs std_srvs ) ament_package() ================================================ FILE: canopen_base_driver/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_base_driver/include/canopen_base_driver/base_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ #define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "canopen_core/driver_node.hpp" namespace ros2_canopen { /** * @brief Abstract Class for a CANopen Device Node * * This class provides the base functionality for creating a * CANopen device node. It provides callbacks for nmt and rpdo. */ class BaseDriver : public ros2_canopen::CanopenDriver { std::shared_ptr> node_canopen_base_driver_; public: BaseDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_base_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_base_driver_->register_rpdo_cb(rpdo_cb); } }; } // namespace ros2_canopen #endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ ================================================ FILE: canopen_base_driver/include/canopen_base_driver/diagnostic_collector.hpp ================================================ // Copyright 2023 ROS-Industrial // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef DIAGNOSTICS_COLLECTOR_HPP_ #define DIAGNOSTICS_COLLECTOR_HPP_ #include #include #include #include #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "diagnostic_updater/publisher.hpp" namespace ros2_canopen { /** * @brief A class to collect diagnostic information * */ class DiagnosticsCollector { public: DiagnosticsCollector() : level_(diagnostic_msgs::msg::DiagnosticStatus::OK) {} /** * @brief Get the Level * Returns the current level (OK, WARN, ERROR or STALE) of the diagnostic * @return unsigned char */ unsigned char getLevel() const { return static_cast(level_.load(std::memory_order_relaxed)); } /** * @brief Get the Message object * Returns the current message of the diagnostic * @return std::string */ std::string getMessage() const { return message_; } /** * @brief Get the Value object * Returns the current different device state values of the diagnostic. * @param key Search device state by key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", * "cia402_mode" etc. * @return std::string */ std::string getValue(const std::string & key) const { std::lock_guard lock(mutex_); auto it = values_.find(key); if (it != values_.end()) return it->second; else return ""; // Return an empty string if key not found } /** * @brief Store current device summary * * @param lvl Operation level (OK, WARN, ERROR or STALE) * @param message Device summary message */ void summary(unsigned char lvl, const std::string & message) { std::lock_guard lock(mutex_); this->setLevel(lvl); this->setMessage(message); } /** * @brief Store current device summary * * @param lvl Operation level (OK, WARN, ERROR or STALE) * @param format Device summary message format * @param ... */ void summaryf(unsigned char lvl, const char * format, ...) { va_list args; va_start(args, format); char buffer[1024]; vsnprintf(buffer, sizeof(buffer), format, args); va_end(args); summary(lvl, std::string(buffer)); } /** * @brief Add a device state value * * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. * @param value Current device state value */ void add(const std::string & key, const std::string & value) { std::lock_guard lock(mutex_); this->setValue(key, value); } /** * @brief Add a device state value * * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. * @param format Current device state value format * @param ... */ void addf(const std::string & key, const char * format, ...) { va_list args; va_start(args, format); char buffer[1024]; vsnprintf(buffer, sizeof(buffer), format, args); va_end(args); add(key, std::string(buffer)); } /** * @brief Update all diagnostic information * * @param lvl Operation level (OK, WARN, ERROR or STALE) * @param message Device summary message * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. * @param value Current device state value */ void updateAll( unsigned char lvl, const std::string & message, const std::string & key, const std::string & value) { std::lock_guard lock(mutex_); this->setLevel(lvl); this->setMessage(message); this->setValue(key, value); } private: std::atomic level_; std::string message_; std::unordered_map values_; mutable std::mutex mutex_; void setLevel(unsigned char lvl) { level_.store(static_cast(lvl), std::memory_order_relaxed); } void setMessage(const std::string & message) { message_ = message; } void setValue(const std::string & key, const std::string & value) { values_[key] = value; } // std::unordered_map getValues() const // { // std::lock_guard lock(mutex_); // return values_; // } }; } // namespace ros2_canopen #endif // DIAGNOSTICS_COLLECTOR_HPP_ ================================================ FILE: canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ #define CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "canopen_core/exchange.hpp" using namespace std::chrono_literals; using namespace lely; namespace ros2_canopen { struct pdo_mapping { bool is_tpdo; bool is_rpdo; }; typedef std::map> PDOMap; class DriverDictionary : public lely::CODev { public: DriverDictionary(std::string eds_file) : lely::CODev(eds_file.c_str()) {} ~DriverDictionary() { // lely::CODev::~CODev(); } std::shared_ptr createPDOMapping() { std::shared_ptr pdo_map = std::make_shared(); fetchRPDO(pdo_map); fetchTPDO(pdo_map); return pdo_map; // COObj * first = co_dev_first_obj((lely::CODev *)this); // COObj * last = co_dev_last_obj((lely::CODev *)this); // if (first == nullptr || last == nullptr) // { // std::cout << "No objects found in dictionary" << std::endl; // return pdo_map; // } // COObj * current = first; // while (current != last) // { // uint16_t index = co_obj_get_idx(current); // auto subids = current->getSubidx(); // bool created = false; // for (auto subid : subids) // { // bool is_rpdo = checkObjRPDO(index, subid); // bool is_tpdo = checkObjTPDO(index, subid); // std::cout << "Found subobject: index=" << std::hex << (int)index // << " subindex=" << (int)subid << (is_rpdo ? " rpdo" : "") // << (is_tpdo ? " tpdo" : "") << std::endl; // if (is_rpdo || is_tpdo) // { // pdo_mapping mapping; // mapping.is_rpdo = is_rpdo; // mapping.is_tpdo = is_tpdo; // if (!created) // { // pdo_map->emplace(index, std::map()); // created = true; // } // (*pdo_map)[index].emplace(subid, mapping); // } // } // current = co_obj_next(current); // } // return pdo_map; } void fetchRPDO(std::shared_ptr map) { for (int index = 0; index < 256; index++) { for (int subindex = 1; subindex < 9; subindex++) { auto obj = find(0x1600 + index, subindex); if (obj == nullptr) { continue; } uint32_t data; { data = obj->getVal(); } uint8_t tmps = (data >> 8) & 0xFF; uint16_t tmpi = (data >> 16) & 0xFFFF; if (tmpi == 0U) { continue; } pdo_mapping mapping; mapping.is_rpdo = true; mapping.is_tpdo = false; (*map)[tmpi][tmps] = mapping; std::cout << "Found rpdo mapped object: index=" << std::hex << (int)tmpi << " subindex=" << (int)tmps << std::endl; } } } void fetchTPDO(std::shared_ptr map) { for (int index = 0; index < 256; index++) { for (int subindex = 1; subindex < 9; subindex++) { auto obj = find(0x1A00 + index, subindex); if (obj == nullptr) { continue; } uint32_t data; { data = obj->getVal(); } uint8_t tmps = (data >> 8) & 0xFF; uint16_t tmpi = (data >> 16) & 0xFFFF; if (tmpi == 0U) { continue; } pdo_mapping mapping; mapping.is_rpdo = false; mapping.is_tpdo = true; (*map)[tmpi][tmps] = mapping; std::cout << "Found tpdo mapped object: index=" << std::hex << (int)tmpi << " subindex=" << (int)tmps << std::endl; } } } bool checkObjRPDO(uint16_t idx, uint8_t subidx) { // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx // << " subindex=" << (int)subidx << std::endl; for (int i = 0; i < 256; i++) { if (this->checkObjInPDO(i, 0x1600, idx, subidx)) { return true; } } return false; } bool checkObjTPDO(uint16_t idx, uint8_t subidx) { // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx // << " subindex=" << (int)subidx << std::endl; for (int i = 0; i < 256; i++) { if (this->checkObjInPDO(i, 0x1A00, idx, subidx)) { return true; } } return false; } bool checkObjInPDO(uint8_t pdo, uint16_t mapping_idx, uint16_t idx, uint8_t subindex) { for (int i = 1; i < 9; i++) { auto obj = find(mapping_idx + pdo, i); if (obj == nullptr) { return false; } uint32_t data; { data = obj->getVal(); } uint8_t tmps = (data >> 8) & 0xFF; uint16_t tmpi = (data >> 16) & 0xFFFF; if (tmps == subindex && tmpi == idx) { std::cout << "Found object in pdo: " << (int)pdo << std::endl; return true; } } return false; } }; enum class LelyBridgeErrc { NotListedDevice = 'A', NoResponseOnDeviceType = 'B', DeviceTypeDifference = 'C', VendorIdDifference = 'D', HeartbeatIssue = 'E', NodeGuardingIssue = 'F', InconsistentProgramDownload = 'G', SoftwareUpdateRequired = 'H', SoftwareDownloadFailed = 'I', ConfigurationDownloadFailed = 'J', StartErrorControlFailed = 'K', NmtSlaveInitiallyOperational = 'L', ProductCodeDifference = 'M', RevisionCodeDifference = 'N', SerialNumberDifference = 'O', TimeOut = 'T' }; struct LelyBridgeErrCategory : std::error_category { const char * name() const noexcept override; std::string message(int ev) const override; }; } // namespace ros2_canopen namespace std { template <> struct is_error_code_enum : true_type { }; std::error_code make_error_code(ros2_canopen::LelyBridgeErrc); } // namespace std namespace ros2_canopen { /** * @brief Lely Driver Bridge * * This class provides functionalities for bridging between * Lelycore drivers and standard C++ functions. This means * it provides async and sync functions for interacting with * CANopen devices using synchronisation functionalities from C++ standard * library. * */ class LelyDriverBridge : public canopen::FiberDriver { protected: // Dictionary for driver based on DCF and BIN files. std::unique_ptr dictionary_; std::mutex dictionary_mutex_; std::shared_ptr pdo_map_; // SDO Read synchronisation items std::shared_ptr> sdo_read_data_promise; std::shared_ptr> sdo_write_data_promise; std::mutex sdo_mutex; bool running; std::condition_variable sdo_cond; // NMT synchronisation items std::promise nmt_state_promise; std::atomic nmt_state_is_set; std::mutex nmt_mtex; // RPDO synchronisation items std::promise rpdo_promise; std::atomic rpdo_is_set; std::mutex pdo_mtex; std::shared_ptr> rpdo_queue; // EMCY synchronisation items std::promise emcy_promise; std::atomic emcy_is_set; std::mutex emcy_mtex; std::shared_ptr> emcy_queue; // BOOT synchronisation items std::atomic booted; char boot_status; std::string boot_what; canopen::NmtState boot_state; std::condition_variable boot_cond; std::mutex boot_mtex; std::chrono::milliseconds boot_timeout_; uint8_t nodeid; std::string name_; std::chrono::milliseconds sdo_timeout; std::function on_sync_function_; // void set_sync_function(std::function on_sync_function) // { // on_sync_function_ = on_sync_function; // } // void unset_sync_function() // { // on_sync_function_ = std::function(); // } void OnSync(uint8_t cnt, const time_point & t) noexcept override { if (on_sync_function_ != nullptr) { try { on_sync_function_(); } catch (...) { } } } /** * @brief OnState Callback * * This callback function is called when an Nmt state * change is detected on the connected device. * * @param [in] state NMT State */ void OnState(canopen::NmtState state) noexcept override; /** * @brief OnBoot Callback * This callback is called when the Boot process of the * slave that was initiated by the master has been success * fully finished. * * @param st * @param es * @param what */ virtual void OnBoot(canopen::NmtState st, char es, const ::std::string & what) noexcept override; /** * @brief OnRpdoWrite Callback * * This callback function is called when an RPDO * write request is received from the connected device. * @todo This function should use a threadsafe queue not the icky implementation we have now. * * @param [in] idx Object Index * @param [in] subidx Object Subindex */ void OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override; /** * The function invoked when an EMCY message is received from the remote node. * @todo This function should use a threadsafe queue not the icky implementation we have now. * * @param eec the emergency error code. * @param er the error register. * @param msef the manufacturer-specific error code. */ void OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept override; // void OnConfig(::std::function res) noexcept override; public: using FiberDriver::FiberDriver; /** * @brief Construct a new Lely Bridge object * * @param [in] exec Executor to use * @param [in] master Master to use * @param [in] id NodeId to connect to * @param [in] eds EDS file * @param [in] bin BIN file (concise dcf) * @param [in] timeout Timeout in milliseconds for SDO reads/writes * */ LelyDriverBridge( ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id, std::string name, std::string eds, std::string bin, std::chrono::milliseconds timeout = 20ms, std::chrono::milliseconds boot_timeout = 20ms) : FiberDriver(exec, master, id), rpdo_queue(new SafeQueue()), emcy_queue(new SafeQueue()) { nodeid = id; running = false; name_ = name; dictionary_ = std::make_unique(eds.c_str()); struct stat buffer; if (stat(bin.c_str(), &buffer) == 0) { co_unsigned16_t * a = NULL; co_unsigned16_t * b = NULL; dictionary_->readDCF(a, b, bin.c_str()); } pdo_map_ = dictionary_->createPDOMapping(); sdo_timeout = timeout; boot_timeout_ = boot_timeout; } bool has_object(uint16_t idx, uint8_t subidx) { std::scoped_lock lck(this->dictionary_mutex_); return this->dictionary_->find(idx, subidx) != nullptr; } /** * @brief Asynchronous SDO Write * * Writes the data passed to the function via SDO to * the connected device. * * @param [in] data Data to written. * * @return std::future * Returns an std::future that is fulfilled * when the write request was done. An error is * stored when the write request was unsuccessful. */ std::future async_sdo_write(COData data); template std::future async_sdo_write_typed(uint16_t idx, uint8_t subidx, T value) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; auto prom = std::make_shared>(); lely::COSub * sub = this->dictionary_->find(idx, subidx); if (sub == nullptr) { std::cout << "async_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx << " object does not exist" << std::endl; prom->set_value(false); this->running = false; this->sdo_cond.notify_one(); return prom->get_future(); } this->SubmitWrite( idx, subidx, value, [this, value, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable { if (ec) { prom->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); } else { std::scoped_lock lck(this->dictionary_mutex_); this->dictionary_->setVal(idx, subidx, value); prom->set_value(true); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, this->sdo_timeout); return prom->get_future(); } template bool sync_sdo_write_typed( uint16_t idx, uint8_t subidx, T value, std::chrono::milliseconds timeout) { auto fut = async_sdo_write_typed(idx, subidx, value); auto wait_res = fut.wait_for(timeout); if (wait_res == std::future_status::timeout) { std::cout << "sync_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx << " timed out." << std::endl; return false; } bool res = false; try { res = fut.get(); } catch (std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger(name_), e.what()); } return res; } /** * @brief Aynchronous SDO Read * * Reads the indicated SDO object from the connected * device. * * @param [in] data Data to be read, the data entry is not used. * @return std::future * Returns an std::future that is fulfilled * when the read request was done. The result of the request * is stored in the future. An error is stored when the read * request was unsuccessful. */ std::future async_sdo_read(COData data); template std::future async_sdo_read_typed(uint16_t idx, uint8_t subidx) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; auto prom = std::make_shared>(); lely::COSub * sub = this->dictionary_->find(idx, subidx); if (sub == nullptr) { std::cout << "async_sdo_read: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx << " object does not exist" << std::endl; try { throw lely::canopen::SdoError(this->get_id(), idx, subidx, lely::canopen::SdoErrc::NO_OBJ); } catch (...) { prom->set_exception(std::current_exception()); } this->running = false; this->sdo_cond.notify_one(); return prom->get_future(); } this->SubmitRead( idx, subidx, [this, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable { if (ec) { prom->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); } else { std::scoped_lock lck(this->dictionary_mutex_); this->dictionary_->setVal(idx, subidx, value); prom->set_value(value); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, this->sdo_timeout); return prom->get_future(); } template bool sync_sdo_read_typed( uint16_t idx, uint8_t subidx, T & value, std::chrono::milliseconds timeout) { auto fut = async_sdo_read_typed(idx, subidx); auto wait_res = fut.wait_for(timeout); if (wait_res == std::future_status::timeout) { std::cout << "sync_sdo_read_typed: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx << " timed out." << std::endl; return false; } bool res = false; try { value = fut.get(); res = true; } catch (std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger(name_), e.what()); res = false; } return res; } /** * @brief Asynchronous request for NMT * * Waits for an NMT state change to occur. The new * state is stored in the future returned by the function. * * @return std::future * The returned future is set when NMT State changes. */ std::future async_request_nmt(); /** * @brief Asynchronous request for RPDO * * Waits for an RPDO write request to be received from * the slave. The content of the request are stored in * the returned future. * @todo This function should use a threadsafe queue not the icky implementation we have now. * * @return std::future * The returned future is set when an RPDO event is detected. */ std::shared_ptr> get_rpdo_queue(); /** * @brief Asynchronous request for EMCY * @todo This function should use a threadsafe queue not the icky implementation we have now. * * @return std::future * The returned future is set when an EMCY event is detected. */ std::shared_ptr> get_emcy_queue(); /** * @brief Executes a TPDO transmission * * This function executes a TPDO transmission. The{false, true} * object specified in the input data is sent if it * is registered as a TPDO with the master. * * @param [in] data Object and data to be written */ void tpdo_transmit(COData data); /** * @brief Executes a NMT Command * * This function sends the NMT command specified as * parameter. * * @param [in] command NMT Command to execute */ void nmt_command(canopen::NmtCommand command); /** * @brief Get the nodeid * * @return uint8_t */ uint8_t get_id(); /** * @brief Wait for device to be booted * * @return true * @return false */ bool wait_for_boot() { if (booted.load()) return true; std::unique_lock lck(boot_mtex); auto status = boot_cond.wait_for(lck, boot_timeout_); if (status == std::cv_status::timeout) { throw std::system_error( static_cast(LelyBridgeErrc::TimeOut), LelyBridgeErrCategory(), "Boot Timeout"); } if ((boot_status != 0) && (boot_status != 'L')) { throw std::system_error(static_cast(boot_status), LelyBridgeErrCategory(), "Boot Issue"); } booted.store(true); return true; } void set_sync_function(std::function on_sync_function) { on_sync_function_ = on_sync_function; } void unset_sync_function() { on_sync_function_ = std::function(); } /** * @brief Request master to boot device * */ void Boot() { booted.store(false); FiberDriver::Boot(); } /** * @brief Indicates if Device is booted * * @return true * @return false */ bool is_booted() { return booted.load(); } template void submit_write(COData data) { T value = 0; std::memcpy(&value, &data.data_, sizeof(value)); this->SubmitWrite( data.index_, data.subindex_, value, [this, value](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable { if (ec) { this->sdo_write_data_promise->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); } else { std::scoped_lock lck(this->dictionary_mutex_); this->dictionary_->setVal(idx, subidx, value); this->sdo_write_data_promise->set_value(true); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, this->sdo_timeout); } template void submit_read(COData data) { this->SubmitRead( data.index_, data.subindex_, [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable { if (ec) { this->sdo_read_data_promise->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); } else { std::scoped_lock lck(this->dictionary_mutex_); this->dictionary_->setVal(idx, subidx, value); COData d = {idx, subidx, 0}; std::memcpy(&d.data_, &value, sizeof(T)); this->sdo_read_data_promise->set_value(d); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, this->sdo_timeout); } template const T universal_get_value(uint16_t index, uint8_t subindex) { T value = 0; bool is_tpdo = false; if (this->pdo_map_->find(index) != this->pdo_map_->end()) { auto object = this->pdo_map_->at(index); if (object.find(subindex) != object.end()) { auto entry = object.at(subindex); is_tpdo = entry.is_tpdo; } } if (!is_tpdo) { if (sync_sdo_read_typed(index, subindex, value, this->sdo_timeout)) { return value; } } std::scoped_lock lck(this->dictionary_mutex_); if (typeid(T) == typeid(uint8_t)) { value = this->dictionary_->getVal(index, subindex); } if (typeid(T) == typeid(uint16_t)) { value = this->dictionary_->getVal(index, subindex); } if (typeid(T) == typeid(uint32_t)) { value = this->dictionary_->getVal(index, subindex); } if (typeid(T) == typeid(int8_t)) { value = this->dictionary_->getVal(index, subindex); } if (typeid(T) == typeid(int16_t)) { value = this->dictionary_->getVal(index, subindex); } if (typeid(T) == typeid(int32_t)) { value = this->dictionary_->getVal(index, subindex); } return value; } template void universal_set_value(uint16_t index, uint8_t subindex, T value) { bool is_rpdo = false; if (this->pdo_map_->find(index) != this->pdo_map_->end()) { auto object = this->pdo_map_->at(index); if (object.find(subindex) != object.end()) { auto entry = object.at(subindex); is_rpdo = entry.is_rpdo; } } if (is_rpdo) { std::scoped_lock lck(this->dictionary_mutex_); this->dictionary_->setVal(index, subindex, value); this->tpdo_mapped[index][subindex] = value; this->tpdo_mapped[index][subindex].WriteEvent(); } else { sync_sdo_write_typed(index, subindex, value, this->sdo_timeout); } } }; } // namespace ros2_canopen #endif // CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ ================================================ FILE: canopen_base_driver/include/canopen_base_driver/lifecycle_base_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ #define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "canopen_core/driver_node.hpp" namespace ros2_canopen { /** * @brief Lifecycle Base Driver * * A very basic driver without any functionality. * */ class LifecycleBaseDriver : public ros2_canopen::LifecycleCanopenDriver { std::shared_ptr> node_canopen_base_driver_; public: LifecycleBaseDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_base_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_base_driver_->register_rpdo_cb(rpdo_cb); } }; } // namespace ros2_canopen #endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ ================================================ FILE: canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_BASE_DRIVER #define NODE_CANOPEN_BASE_DRIVER #include "canopen_base_driver/diagnostic_collector.hpp" #include "canopen_base_driver/lely_driver_bridge.hpp" #include "canopen_core/node_interfaces/node_canopen_driver.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/trigger.hpp" #include "canopen_interfaces/msg/co_data.hpp" #include "canopen_interfaces/srv/co_read.hpp" #include "canopen_interfaces/srv/co_write.hpp" namespace ros2_canopen { namespace node_interfaces { template class NodeCanopenBaseDriver : public NodeCanopenDriver { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: std::thread nmt_state_publisher_thread_; std::thread rpdo_publisher_thread_; std::thread emcy_publisher_thread_; std::mutex driver_mutex_; std::shared_ptr lely_driver_; uint32_t period_ms_; int sdo_timeout_ms_; int boot_timeout_ms_; bool polling_; // nmt state callback std::function nmt_state_cb_; // rpdo callback std::function rpdo_cb_; // emcy callback std::function emcy_cb_; std::shared_ptr> emcy_queue_; std::shared_ptr> rpdo_queue_; rclcpp::TimerBase::SharedPtr poll_timer_; // Diagnostic components std::atomic diagnostic_enabled_; uint32_t diagnostic_period_ms_; std::shared_ptr diagnostic_updater_; std::shared_ptr diagnostic_collector_; virtual void poll_timer_callback(); void nmt_listener(); virtual void on_nmt(canopen::NmtState nmt_state); void rdpo_listener(); virtual void on_rpdo(COData data); void emcy_listener(); virtual void on_emcy(COEmcy emcy); virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); public: NodeCanopenBaseDriver(NODETYPE * node); virtual ~NodeCanopenBaseDriver() { if (nmt_state_publisher_thread_.joinable()) { nmt_state_publisher_thread_.join(); } if (rpdo_publisher_thread_.joinable()) { rpdo_publisher_thread_.join(); } } virtual void init(bool called_from_base); virtual void configure(bool called_from_base); virtual void activate(bool called_from_base); virtual void deactivate(bool called_from_base); virtual void cleanup(bool called_from_base); virtual void shutdown(bool called_from_base); virtual void add_to_master(); virtual void remove_from_master(); /** * @brief Register a callback for NMT state change * * @param nmt_state_cb */ void register_nmt_state_cb(std::function nmt_state_cb) { nmt_state_cb_ = std::move(nmt_state_cb); } /** * @brief Register a callback for RPDO * * @param rpdo_cb */ void register_rpdo_cb(std::function rpdo_cb) { rpdo_cb_ = std::move(rpdo_cb); } /** * @brief Register a callback for EMCY * * @param emcy_cb */ void register_emcy_cb(std::function emcy_cb) { emcy_cb_ = std::move(emcy_cb); } }; typedef NodeCanopenBaseDriver NCBDNode; typedef NodeCanopenBaseDriver NCBDLifecycleNode; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_BASE_DRIVER_IMPL #define NODE_CANOPEN_BASE_DRIVER_IMPL #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "canopen_core/driver_error.hpp" using namespace ros2_canopen::node_interfaces; template NodeCanopenBaseDriver::NodeCanopenBaseDriver(NODETYPE * node) : ros2_canopen::node_interfaces::NodeCanopenDriver(node), diagnostic_enabled_(false), diagnostic_collector_(new DiagnosticsCollector()) { } template void NodeCanopenBaseDriver::init(bool called_from_base) { } template <> void NodeCanopenBaseDriver::configure(bool called_from_base) { try { this->non_transmit_timeout_ = std::chrono::milliseconds(this->config_["non_transmit_timeout"].as()); } catch (...) { } RCLCPP_INFO_STREAM( this->node_->get_logger(), "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); try { polling_ = this->config_["polling"].as(); } catch (...) { RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true."); polling_ = true; } if (polling_) { try { period_ms_ = this->config_["period"].as(); } catch (...) { RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms"); period_ms_ = 10; } } // Diagnostic components try { diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as(); } catch (...) { RCLCPP_WARN( this->node_->get_logger(), "Could not read enable diagnostics from config, setting to false."); diagnostic_enabled_ = false; } if (diagnostic_enabled_.load()) { try { diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as(); } catch (...) { RCLCPP_ERROR( this->node_->get_logger(), "Could not read diagnostics period from config, setting to 1000ms"); diagnostic_period_ms_ = 1000; } diagnostic_updater_ = std::make_shared(this->node_); diagnostic_updater_->setHardwareID(std::to_string(this->node_id_)); } std::optional sdo_timeout_ms; try { sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as()); } catch (...) { } sdo_timeout_ms_ = sdo_timeout_ms.value_or(20); std::optional boot_timeout_ms; try { boot_timeout_ms = std::optional(this->config_["boot_timeout_ms"].as()); } catch (...) { } boot_timeout_ms_ = boot_timeout_ms.value_or(1000); } template <> void NodeCanopenBaseDriver::configure(bool called_from_base) { try { this->non_transmit_timeout_ = std::chrono::milliseconds(this->config_["non_transmit_timeout"].as()); } catch (...) { } RCLCPP_INFO_STREAM( this->node_->get_logger(), "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); try { polling_ = this->config_["polling"].as(); } catch (...) { RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true."); polling_ = true; } if (polling_) { try { period_ms_ = this->config_["period"].as(); } catch (...) { RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms"); period_ms_ = 10; } } // Diagnostic components try { diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as(); } catch (...) { RCLCPP_WARN( this->node_->get_logger(), "Could not read enable diagnostics from config, setting to false."); diagnostic_enabled_ = false; } if (diagnostic_enabled_.load()) { try { diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as(); } catch (...) { RCLCPP_WARN( this->node_->get_logger(), "Could not read diagnostics period from config, setting to 1000ms"); diagnostic_period_ms_ = 1000; } diagnostic_updater_ = std::make_shared(this->node_); diagnostic_updater_->setHardwareID(std::to_string(this->node_id_)); } std::optional sdo_timeout_ms; try { sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as()); } catch (...) { } sdo_timeout_ms_ = sdo_timeout_ms.value_or(20); std::optional boot_timeout_ms; try { boot_timeout_ms = std::optional(this->config_["boot_timeout_ms"].as()); } catch (...) { } boot_timeout_ms_ = boot_timeout_ms.value_or(1000); } template void NodeCanopenBaseDriver::activate(bool called_from_base) { nmt_state_publisher_thread_ = std::thread(std::bind(&NodeCanopenBaseDriver::nmt_listener, this)); emcy_queue_ = this->lely_driver_->get_emcy_queue(); rpdo_queue_ = this->lely_driver_->get_rpdo_queue(); if (polling_) { RCLCPP_INFO(this->node_->get_logger(), "Starting with polling mode."); poll_timer_ = this->node_->create_wall_timer( std::chrono::milliseconds(period_ms_), std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this), this->timer_cbg_); } else { RCLCPP_INFO(this->node_->get_logger(), "Starting with event mode."); this->lely_driver_->set_sync_function( std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); } if (diagnostic_enabled_.load()) { RCLCPP_INFO(this->node_->get_logger(), "Starting with diagnostics enabled."); diagnostic_updater_->add( "diagnostic updater", this, &NodeCanopenBaseDriver::diagnostic_callback); } } template void NodeCanopenBaseDriver::deactivate(bool called_from_base) { nmt_state_publisher_thread_.join(); if (poll_timer_) { poll_timer_->cancel(); } emcy_queue_.reset(); rpdo_queue_.reset(); if (diagnostic_enabled_.load()) { diagnostic_updater_->removeByName("diagnostic updater"); } } template void NodeCanopenBaseDriver::cleanup(bool called_from_base) { NodeCanopenDriver::cleanup(called_from_base); // this->lely_driver_->unset_sync_function(); } template void NodeCanopenBaseDriver::shutdown(bool called_from_base) { } template void NodeCanopenBaseDriver::add_to_master() { RCLCPP_INFO(this->node_->get_logger(), "eds file %s", this->eds_.c_str()); RCLCPP_INFO(this->node_->get_logger(), "bin file %s", this->bin_.c_str()); std::shared_ptr>> prom; prom = std::make_shared>>(); std::future> f = prom->get_future(); this->exec_->post( [this, prom]() { std::scoped_lock lock(this->driver_mutex_); this->lely_driver_ = std::make_shared( *(this->exec_), *(this->master_), this->node_id_, this->node_->get_name(), this->eds_, this->bin_, std::chrono::milliseconds(this->sdo_timeout_ms_), std::chrono::milliseconds(this->boot_timeout_ms_)); this->driver_ = std::static_pointer_cast(this->lely_driver_); prom->set_value(lely_driver_); }); auto future_status = f.wait_for(this->non_transmit_timeout_); if (future_status != std::future_status::ready) { RCLCPP_ERROR(this->node_->get_logger(), "Adding timed out."); throw DriverException("add_to_master: adding timed out"); } this->lely_driver_ = f.get(); this->driver_ = std::static_pointer_cast(this->lely_driver_); if (!this->lely_driver_->IsReady()) { bool boot_success = false; int boot_attempts = 0; const int max_boot_attempts = 3; // 1 retry allowed RCLCPP_WARN(this->node_->get_logger(), "Wait for device to boot..."); while (!boot_success && boot_attempts < max_boot_attempts) { boot_attempts++; try { this->lely_driver_->wait_for_boot(); // This will block and throw on failure boot_success = true; RCLCPP_INFO(this->node_->get_logger(), "Driver booted and ready."); } catch (const std::exception & e) { RCLCPP_ERROR( this->node_->get_logger(), "Boot attempt %d failed: %s", boot_attempts, e.what()); if (boot_attempts < max_boot_attempts) { RCLCPP_INFO(this->node_->get_logger(), "Sending NMT reset before retrying boot"); this->lely_driver_->nmt_command(canopen::NmtCommand::RESET_NODE); this->lely_driver_->Boot(); // Trigger boot again RCLCPP_WARN(this->node_->get_logger(), "Retrying boot configuration..."); } else { RCLCPP_ERROR(this->node_->get_logger(), "Boot failed after %d attempts", boot_attempts); } } } if (!boot_success) { throw DriverException( std::string("Boot failed after ") + std::to_string(boot_attempts) + " attempts"); } } if (diagnostic_enabled_.load()) { diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "Device ready", "DEVICE", "Added to master."); } } template void NodeCanopenBaseDriver::remove_from_master() { std::shared_ptr> prom = std::make_shared>(); auto f = prom->get_future(); this->exec_->post( [this, prom]() { this->driver_.reset(); this->lely_driver_.reset(); prom->set_value(); }); auto future_status = f.wait_for(this->non_transmit_timeout_); if (future_status != std::future_status::ready) { throw DriverException("remove_from_master: removing timed out"); } if (diagnostic_enabled_.load()) { diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Device removed", "DEVICE", "Removed from master."); } } template void NodeCanopenBaseDriver::nmt_listener() { while (rclcpp::ok()) { std::future f; { std::scoped_lock lock(this->driver_mutex_); f = this->lely_driver_->async_request_nmt(); } while (f.wait_for(this->non_transmit_timeout_) != std::future_status::ready) { if (!this->activated_.load()) return; } try { auto state = f.get(); if (nmt_state_cb_) { nmt_state_cb_(state, this->lely_driver_->get_id()); } on_nmt(state); } catch (const std::future_error & e) { break; } } } template void NodeCanopenBaseDriver::on_nmt(canopen::NmtState nmt_state) { } template void NodeCanopenBaseDriver::on_rpdo(COData data) { } template void NodeCanopenBaseDriver::on_emcy(COEmcy emcy) { diagnostic_collector_->summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Emergency message received"); std::string emcy_msg = "Emergency message: "; emcy_msg.append("eec: "); emcy_msg.append(std::to_string(emcy.eec)); emcy_msg.append(" er: "); emcy_msg.append(std::to_string(emcy.er)); emcy_msg.append(" msef: "); for (auto & msef : emcy.msef) { emcy_msg.append(std::to_string(msef)); emcy_msg.append(" "); } diagnostic_collector_->add("EMCY", emcy_msg); } template void NodeCanopenBaseDriver::rdpo_listener() { RCLCPP_INFO(this->node_->get_logger(), "Starting RPDO Listener"); auto q = lely_driver_->get_rpdo_queue(); while (rclcpp::ok()) { ros2_canopen::COData rpdo; while (!q->wait_and_pop_for(this->non_transmit_timeout_, rpdo)) { if (!this->activated_.load()) return; } try { if (rpdo_cb_) { rpdo_cb_(rpdo, this->lely_driver_->get_id()); } on_rpdo(rpdo); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Listener error: " << e.what()); break; } } } template void NodeCanopenBaseDriver::poll_timer_callback() { for (int i = 0; i < 10; i++) { auto opt = emcy_queue_->try_pop(); if (!opt.has_value()) { break; } try { if (emcy_cb_) { emcy_cb_(opt.value(), this->lely_driver_->get_id()); } on_emcy(opt.value()); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY poll error: " << e.what()); break; } } for (int i = 0; i < 10; i++) { auto opt = rpdo_queue_->try_pop(); if (!opt.has_value()) { break; } try { if (rpdo_cb_) { rpdo_cb_(opt.value(), this->lely_driver_->get_id()); } on_rpdo(opt.value()); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Poll error: " << e.what()); break; } } } template void NodeCanopenBaseDriver::emcy_listener() { RCLCPP_INFO(this->node_->get_logger(), "Starting EMCY Listener"); auto q = lely_driver_->get_emcy_queue(); while (rclcpp::ok()) { ros2_canopen::COEmcy emcy; while (!q->wait_and_pop_for(this->non_transmit_timeout_, emcy)) { if (!this->activated_.load()) return; } try { if (emcy_cb_) { emcy_cb_(emcy, this->lely_driver_->get_id()); } on_emcy(emcy); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY Listener error: " << e.what()); break; } } } template void NodeCanopenBaseDriver::diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { } #endif ================================================ FILE: canopen_base_driver/include/canopen_base_driver/visibility_control.h ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_BASE_DRIVER__VISIBILITY_CONTROL_H_ #define CANOPEN_BASE_DRIVER__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_BASE_DRIVER_EXPORT __attribute__((dllexport)) #define CANOPEN_BASE_DRIVER_IMPORT __attribute__((dllimport)) #else #define CANOPEN_BASE_DRIVER_EXPORT __declspec(dllexport) #define CANOPEN_BASE_DRIVER_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_BASE_DRIVER_BUILDING_LIBRARY #define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_EXPORT #else #define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_IMPORT #endif #define CANOPEN_BASE_DRIVER_PUBLIC_TYPE CANOPEN_BASE_DRIVER_PUBLIC #define CANOPEN_BASE_DRIVER_LOCAL #else #define CANOPEN_BASE_DRIVER_EXPORT __attribute__((visibility("default"))) #define CANOPEN_BASE_DRIVER_IMPORT #if __GNUC__ >= 4 #define CANOPEN_BASE_DRIVER_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_BASE_DRIVER_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_BASE_DRIVER_PUBLIC #define CANOPEN_BASE_DRIVER_LOCAL #endif #define CANOPEN_BASE_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_BASE_DRIVER__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_base_driver/package.xml ================================================ canopen_base_driver 0.3.2 Library containing abstract CANopen driver class for ros2_canopen Christoph Hellmann Santos Apache-2.0 ament_cmake_ros canopen_core canopen_interfaces lely_core_libraries rclcpp rclcpp_components rclcpp_lifecycle std_msgs std_srvs boost diagnostic_updater ament_lint_auto ament_cmake ================================================ FILE: canopen_base_driver/src/base_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_base_driver/base_driver.hpp" using namespace ros2_canopen; BaseDriver::BaseDriver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) { node_canopen_base_driver_ = std::make_shared>(this); node_canopen_driver_ = std::static_pointer_cast( node_canopen_base_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::BaseDriver) ================================================ FILE: canopen_base_driver/src/lely_driver_bridge.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. // #include "canopen_base_driver/lely_driver_bridge.hpp" #include #include const ros2_canopen::LelyBridgeErrCategory LelyBridgeErrCategoryInstance{}; std::error_code std::make_error_code(ros2_canopen::LelyBridgeErrc e) { return {static_cast(e), LelyBridgeErrCategoryInstance}; } namespace ros2_canopen { const char * LelyBridgeErrCategory::name() const noexcept { return "LelyBridgeError"; } std::string LelyBridgeErrCategory::message(int ev) const { switch (static_cast(ev)) { case LelyBridgeErrc::NotListedDevice: return "The CANopen device is not listed in object 1F81."; case LelyBridgeErrc::NoResponseOnDeviceType: return "No response received for upload request of object 1000."; case LelyBridgeErrc::DeviceTypeDifference: return "Value of object 1000 from CANopen device is different to value in object 1F84 " "(Device type)."; case LelyBridgeErrc::VendorIdDifference: return "Value of object 1018:01 from CANopen device is different to value in object 1F85 " "(Vendor-ID)."; case LelyBridgeErrc::HeartbeatIssue: return "Heartbeat event. No heartbeat message received from CANopen device."; case LelyBridgeErrc::NodeGuardingIssue: return "Node guarding event. No confirmation for guarding request received from CANopen " "device."; case LelyBridgeErrc::InconsistentProgramDownload: return "Objects for program download are not configured or inconsistent."; case LelyBridgeErrc::SoftwareUpdateRequired: return "Software update is required, but not allowed because of configuration or current " "status."; case LelyBridgeErrc::SoftwareDownloadFailed: return "Software update is required, but program download failed."; case LelyBridgeErrc::ConfigurationDownloadFailed: return "Configuration download failed."; case LelyBridgeErrc::StartErrorControlFailed: return "Heartbeat event during start error control service. No heartbeat message received " "from CANopen device during start error control service."; case LelyBridgeErrc::NmtSlaveInitiallyOperational: return "NMT slave was initially operational. (CANopen manager may resume operation with " "other CANopen devices)"; case LelyBridgeErrc::ProductCodeDifference: return "Value of object 1018:02 from CANopen device is different to value in object 1F86 " "(Product code)."; case LelyBridgeErrc::RevisionCodeDifference: return "Value of object 1018:03 from CANopen device is different to value in object 1F87 " "(Revision number)."; case LelyBridgeErrc::SerialNumberDifference: return "Value of object 1018:04 from CANopen device is different to value in object 1F88 " "(Serial number)."; case LelyBridgeErrc::TimeOut: return "The boot configure process timeout!"; default: return "(unrecognized error)"; } } void LelyDriverBridge::OnState(canopen::NmtState state) noexcept { canopen::NmtState st = state; // We assume 1F80 bit 2 is false. All slaves are put into Operational after boot-up. // Lelycore does not track NMT states in this mode except BOOTUP. if (st == canopen::NmtState::BOOTUP) { st = canopen::NmtState::START; } if (!nmt_state_is_set.load()) { // We do not care so much about missing a message, rather push them through. std::unique_lock lk(nmt_mtex, std::defer_lock); if (lk.try_lock()) { nmt_state_is_set.store(true); nmt_state_promise.set_value(st); } } } void LelyDriverBridge::OnBoot(canopen::NmtState st, char es, const ::std::string & what) noexcept { FiberDriver::OnBoot(st, es, what); if (es == 0) { booted.store(true); } std::unique_lock lck(boot_mtex); this->boot_state = st; this->boot_status = es; this->boot_what = what; boot_cond.notify_all(); } // void LelyDriverBridge::OnConfig(::std::function res) noexcept // { // std::cout << "OnConfig" << std::endl; // this->SubmitWait( // std::chrono::seconds(10), // [this, res](std::error_code ec) // { // std::cout << "OnConfig" << std::endl; // FiberDriver::OnConfig(res); // }); // } void LelyDriverBridge::OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept { lely::COSub * sub = this->dictionary_->find(idx, subidx); if (sub == nullptr) { std::cout << "OnRpdoWrite: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx << " object does not exist" << std::endl; return; } uint8_t co_def = (uint8_t)sub->getType(); uint32_t data = 0; switch (co_def) { case CO_DEFTYPE_BOOLEAN: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((bool)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 1); break; } case CO_DEFTYPE_UNSIGNED8: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((uint8_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 1); break; } case CO_DEFTYPE_INTEGER8: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((int8_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 1); break; } case CO_DEFTYPE_UNSIGNED16: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((uint16_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 2); break; } case CO_DEFTYPE_INTEGER16: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((int16_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 2); break; } case CO_DEFTYPE_UNSIGNED32: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((uint32_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 4); break; } case CO_DEFTYPE_INTEGER32: { std::scoped_lock lck(this->dictionary_mutex_); sub->setVal((int32_t)rpdo_mapped[idx][subidx]); std::memcpy(&data, &sub->getVal(), 4); break; } default: std::cerr << "Unsupported CO_DEFTYPE: " << std::hex << (unsigned int)co_def << std::endl; break; } COData codata = {idx, subidx, data}; // We do not care so much about missing a message, rather push them through. rpdo_queue->push(codata); } void LelyDriverBridge::OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept { FiberDriver::OnEmcy(eec, er, msef); COEmcy emcy; emcy.eec = eec; emcy.er = er; for (int i = 0; i < 5; i++) emcy.msef[i] = msef[i]; emcy_queue->push(emcy); } std::future LelyDriverBridge::async_sdo_write(COData data) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; sdo_write_data_promise = std::make_shared>(); lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); if (sub == nullptr) { std::cout << "async_sdo_write: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ << " object does not exist" << std::endl; this->sdo_write_data_promise->set_value(false); this->running = false; return sdo_write_data_promise->get_future(); } uint8_t co_def = (uint8_t)sub->getType(); try { switch (co_def) { case CO_DEFTYPE_BOOLEAN: { this->submit_write(data); break; } case CO_DEFTYPE_UNSIGNED8: { this->submit_write(data); break; } case CO_DEFTYPE_INTEGER8: { this->submit_write(data); break; } case CO_DEFTYPE_UNSIGNED16: { this->submit_write(data); break; } case CO_DEFTYPE_INTEGER16: { this->submit_write(data); break; } case CO_DEFTYPE_UNSIGNED32: { this->submit_write(data); break; } case CO_DEFTYPE_INTEGER32: { this->submit_write(data); break; } default: std::cerr << "Unsupported CO_DEFTYPE: " << std::hex << (unsigned int)co_def << std::endl; break; } } catch (lely::canopen::SdoError & e) { this->sdo_read_data_promise->set_exception(lely::canopen::make_sdo_exception_ptr( this->get_id(), data.index_, data.subindex_, e.code(), "AsyncUpload")); this->running = false; this->sdo_cond.notify_one(); } return sdo_write_data_promise->get_future(); } std::future LelyDriverBridge::async_sdo_read(COData data) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; sdo_read_data_promise = std::make_shared>(); lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); if (sub == nullptr) { std::cout << "async_sdo_read: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ << " object does not exist" << std::endl; try { throw lely::canopen::SdoError( this->get_id(), data.index_, data.subindex_, lely::canopen::SdoErrc::NO_OBJ); } catch (...) { this->sdo_read_data_promise->set_exception(std::current_exception()); } this->running = false; return sdo_read_data_promise->get_future(); } uint8_t co_def = (uint8_t)sub->getType(); try { if (co_def == CO_DEFTYPE_BOOLEAN) { this->submit_read(data); } if (co_def == CO_DEFTYPE_UNSIGNED8) { this->submit_read(data); } if (co_def == CO_DEFTYPE_INTEGER8) { this->submit_read(data); } if (co_def == CO_DEFTYPE_UNSIGNED16) { this->submit_read(data); } if (co_def == CO_DEFTYPE_INTEGER16) { this->submit_read(data); } if (co_def == CO_DEFTYPE_UNSIGNED32) { this->submit_read(data); } if (co_def == CO_DEFTYPE_INTEGER32) { this->submit_read(data); } } catch (lely::canopen::SdoError & e) { this->sdo_read_data_promise->set_exception(lely::canopen::make_sdo_exception_ptr( this->get_id(), data.index_, data.subindex_, e.code(), "AsyncUpload")); this->running = false; this->sdo_cond.notify_one(); } return sdo_read_data_promise->get_future(); } std::future LelyDriverBridge::async_request_nmt() { std::scoped_lock lk(nmt_mtex); nmt_state_is_set.store(false); nmt_state_promise = std::promise(); return nmt_state_promise.get_future(); } std::shared_ptr> LelyDriverBridge::get_rpdo_queue() { return rpdo_queue; } std::shared_ptr> LelyDriverBridge::get_emcy_queue() { return emcy_queue; } void LelyDriverBridge::tpdo_transmit(COData data) { lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); if (sub == nullptr) { std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ << " object does not exist" << std::endl; return; } uint8_t co_def = (uint8_t)sub->getType(); try { switch (co_def) { case CO_DEFTYPE_BOOLEAN: { bool val; std::memcpy(&val, &data.data_, sizeof(bool)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_UNSIGNED8: { uint8_t val; std::memcpy(&val, &data.data_, sizeof(uint8_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_INTEGER8: { int8_t val; std::memcpy(&val, &data.data_, sizeof(int8_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_UNSIGNED16: { uint16_t val; std::memcpy(&val, &data.data_, sizeof(uint16_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_INTEGER16: { int16_t val; std::memcpy(&val, &data.data_, sizeof(int16_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_UNSIGNED32: { uint32_t val; std::memcpy(&val, &data.data_, sizeof(uint32_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } case CO_DEFTYPE_INTEGER32: { int32_t val; std::memcpy(&val, &data.data_, sizeof(int32_t)); tpdo_mapped[data.index_][data.subindex_] = val; std::scoped_lock lck(this->dictionary_mutex_); sub->setVal(val); break; } default: std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ << " type=" << std::hex << (unsigned int)co_def << " data:" << (uint32_t)data.data_ << "TYPE NOT IMPLEMENTED - DATA WILL NOT BE WRTITTEN!!" << std::endl; break; } tpdo_mapped[data.index_][data.subindex_].WriteEvent(); // Reduce the amount of output - this is not needed as it is on every write // std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex // << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ // << " type=" << std::hex << (unsigned int)co_def << " data:" << (uint32_t)data.data_ // << std::endl; } catch (lely::canopen::SdoError & e) { std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ << " type=" << std::hex << (unsigned int)co_def << e.what() << std::endl; return; } } void LelyDriverBridge::nmt_command(canopen::NmtCommand command) { this->master.Command(command, nodeid); } uint8_t LelyDriverBridge::get_id() { return nodeid; } } // namespace ros2_canopen ================================================ FILE: canopen_base_driver/src/lifecycle_base_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_base_driver/lifecycle_base_driver.hpp" using namespace ros2_canopen; LifecycleBaseDriver::LifecycleBaseDriver(rclcpp::NodeOptions node_options) : LifecycleCanopenDriver(node_options) { node_canopen_base_driver_ = std::make_shared>(this); node_canopen_driver_ = std::static_pointer_cast( node_canopen_base_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleBaseDriver) ================================================ FILE: canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp" #include "canopen_core/driver_error.hpp" using namespace ros2_canopen::node_interfaces; template class ros2_canopen::node_interfaces::NodeCanopenBaseDriver; template class ros2_canopen::node_interfaces::NodeCanopenBaseDriver< rclcpp_lifecycle::LifecycleNode>; ================================================ FILE: canopen_base_driver/test/CMakeLists.txt ================================================ ament_add_gtest(test_node_canopen_base_driver_ros test_node_canopen_base_driver_ros.cpp ) target_include_directories(test_node_canopen_base_driver_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_canopen_base_driver_ros ${canopen_interfaces_TARGETS} ${std_msgs_TARGETS} ${std_srvs_TARGETS} Boost::thread canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master diagnostic_updater::diagnostic_updater node_canopen_base_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gtest(test_base_driver_component test_base_driver_component.cpp ) target_include_directories(test_base_driver_component PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_base_driver_component ${canopen_interfaces_TARGETS} ${std_msgs_TARGETS} ${std_srvs_TARGETS} Boost::thread canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master diagnostic_updater::diagnostic_updater rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) ================================================ FILE: canopen_base_driver/test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=2 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=32 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1A00 20=0x1A01 21=0x1F25 22=0x1F55 23=0x1F80 24=0x1F81 25=0x1F82 26=0x1F84 27=0x1F85 28=0x1F86 29=0x1F87 30=0x1F88 31=0x1F89 32=0x1F8A [ManufacturerObjects] SupportedObjects=12 1=0x2000 2=0x2001 3=0x2200 4=0x2201 5=0x5800 6=0x5801 7=0x5A00 8=0x5A01 9=0x5C00 10=0x5C01 11=0x5E00 12=0x5E01 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=2 2=0x00000082 3=0x00000083 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=2 2=0x00000005 3=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=2 2=0x00000360 3=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=proxy_device_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=proxy_device_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=proxy_device_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=proxy_device_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_base_driver/test/test_base_driver_component.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include #include #include #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "gtest/gtest.h" using namespace rclcpp_components; TEST(ComponentLoad, test_load_component_1) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_base_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[0]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } TEST(ComponentLoad, test_load_component_2) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_base_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[1]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } ================================================ FILE: canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "gtest/gtest.h" TEST(NodeCanopenBaseDriver, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp::Node * node = new rclcpp::Node("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " "\"simple.eds\"\ndcf_path: \"\"\n"); node->set_parameter(container_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); // Can't activate as master cannot be set. EXPECT_ANY_THROW(iface->activate()); iface->shutdown(); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " "\"simple.eds\"\ndcf_path: \"\"\n"); node->set_parameter(container_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); // Can't activate as master cannot be set. EXPECT_ANY_THROW(iface->activate()); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } ================================================ FILE: canopen_core/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.2.9 (2024-04-16) ------------------ * Add timeouts * Contributors: Vishnuprasad Prachandabhanu, ipa-vsp 0.3.2 (2025-12-05) ------------------ 0.3.1 (2025-06-23) ------------------ * Add namespacing support * Contributors: Christoph Hellmann Santos, Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * Merge pull request `#270 `_ from gsalinas/can-namespace-pr Put components loaded by the device container into its namespace, if any. * pre-commit update * Put components loaded by the device container into its namespace, if any. * Merge pull request `#280 `_ from ipa-vsp/fix/yaml-build-error Fix undefined reference to yaml library * fix undefined reference to yaml * Fix logging in device_container.cpp (`#277 `_) * 0.2.9 * forthcoming * Merge pull request `#220 `_ from ipa-vsp/feature/timeout-config Add timeouts * change from 100ms to 2000ms * non transmit time from bus.yml * timeout for booting slave * Contributors: Christoph Hellmann Santos, Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix master and driver lifecycle * Fix QoS build warning in canopen_core * Use consistenlty HEX output for NodeID and Index. * Contributors: Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Aulon Bajrami, Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_core/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.5) project(canopen_core) # Default to C99 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") # find dependencies find_package(ament_cmake REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(lely_core_libraries REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(yaml_cpp_vendor REQUIRED) include(ConfigExtras.cmake) add_library(node_canopen_driver SHARED src/node_interfaces/node_canopen_driver.cpp src/driver_error.cpp src/driver_node.cpp ) target_compile_features(node_canopen_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_driver PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(node_canopen_driver PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(node_canopen_driver PUBLIC ${canopen_interfaces_TARGETS} ${lely_core_libraries_LIBRARIES} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) add_library(node_canopen_master SHARED src/node_interfaces/node_canopen_master.cpp src/master_error.cpp src/master_node.cpp ) target_compile_features(node_canopen_master PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_master PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(node_canopen_master PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(node_canopen_master PUBLIC ${canopen_interfaces_TARGETS} ${lely_core_libraries_LIBRARIES} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) add_library(device_container SHARED src/device_container.cpp src/configuration_manager.cpp src/device_container_error.cpp src/lifecycle_manager.cpp ) target_compile_features(device_container PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(device_container PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(device_container PUBLIC $ $ ) target_link_libraries(device_container PUBLIC ${canopen_interfaces_TARGETS} node_canopen_driver node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) add_executable(device_container_node src/device_container_node.cpp ) target_link_libraries(device_container_node PUBLIC ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} device_container node_canopen_driver node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) target_include_directories(device_container_node PUBLIC "$" "$" ${rclcpp_INCLUDE_DIRS} ) install( DIRECTORY include/ DESTINATION include ) install( DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/ ) install( TARGETS node_canopen_driver node_canopen_master device_container EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(TARGETS device_container_node DESTINATION lib/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( node_canopen_driver node_canopen_master ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( Boost canopen_interfaces lely_core_libraries lifecycle_msgs rclcpp rclcpp_components rclcpp_lifecycle yaml_cpp_vendor ) # https://github.com/ament/ament_cmake/blob/e78ed7e084489c2a48cb91dec9da92c13e9653c9/ament_cmake_core/cmake/core/ament_package.cmake#L24-L32 ament_package(CONFIG_EXTRAS ConfigExtras.cmake) ================================================ FILE: canopen_core/ConfigExtras.cmake ================================================ # Copyright (c) 2023 Vishnuprasad Prachandabhanu # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # find_package(Boost REQUIRED system thread) ================================================ FILE: canopen_core/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_core/include/canopen_core/configuration_manager.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CONFIGURATION_MANAGER_HPP #define CONFIGURATION_MANAGER_HPP #include #include #include #include #include #include #include "yaml-cpp/yaml.h" namespace ros2_canopen { /** * @brief Manager for Bus Configuration. * * The Bus configuration Manager stores the YAML bus configuration and * enables reading configuration entries. The configuration manager is passed * to all ros2_canopen master and slave drivers to enable reading driver specific * configuration parameters from the YAML configuration file. * */ class ConfigurationManager { private: std::string file_; ///< Stores the configuration file name YAML::Node root_; ///< Stores YAML root node std::map devices_; ///< Stores all configuration per device public: ConfigurationManager(std::string & file) : file_(file) { root_ = YAML::LoadFile(file_.c_str()); } /** * @brief Gets a configuration entry for a specific device * * @tparam T Datatype of the retrieved object * @param device_name Device name * @param entry_name Entry name * @return std::optional Return value, can be empty. */ template std::optional get_entry(std::string device_name, std::string entry_name) { try { auto config = devices_.at(device_name); return std::optional(config[entry_name.c_str()].as()); } catch (const std::exception & e) { RCLCPP_INFO( rclcpp::get_logger("yaml-cpp"), "Failed to load entry \"%s\" for device \"%s\" ", entry_name.c_str(), device_name.c_str()); } return std::nullopt; } /** * @brief Dump device string * * @param device_name * @return std::string */ std::string dump_device(std::string device_name) { std::string result; try { auto config = devices_.at(device_name); result = YAML::Dump(config); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } return result; } /** * @brief Initialises the configuration. * */ void init_config(); /** * @brief Returns all device names * * @param devices List with names of all devices * @return uint32_t Number of devices discovered */ uint32_t get_all_devices(std::vector & devices); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/device_container.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef LIFECYCLE_DEVICE_CONTAINER_NODE_HPP #define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP #include #include #include #include #include #include "canopen_core/configuration_manager.hpp" #include "canopen_core/driver_node.hpp" #include "canopen_core/lifecycle_manager.hpp" #include "canopen_core/master_node.hpp" #include "canopen_interfaces/srv/co_node.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" using namespace std::chrono_literals; namespace ros2_canopen { /** * @brief Lifecycle Device Container for CANopen * * This class provides the functionality for loading a * the CANopen Master and CANopen Device Drivers based on the * Configuration Files. Configuration files need to be passed in * as parameters. * */ class DeviceContainer : public rclcpp_components::ComponentManager { public: /** * @brief Construct a new Lifecycle Device Container Node object * * @param [in] executor The executor to add loaded master and devices to. * @param [in] node_name The name of the node * @param [in] node_options Passed to the device_container node */ DeviceContainer( std::weak_ptr executor = std::weak_ptr(), std::string node_name = "device_container", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp_components::ComponentManager(executor, node_name, node_options) { executor_ = executor; this->declare_parameter("can_interface_name", ""); this->declare_parameter("master_config", ""); this->declare_parameter("bus_config", ""); this->declare_parameter("master_bin", ""); client_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); init_driver_service_ = this->create_service( "~/init_driver", std::bind( &DeviceContainer::on_init_driver, this, std::placeholders::_1, std::placeholders::_2), rclcpp::QoS(10), client_cbg_); this->loadNode_srv_.reset(); this->unloadNode_srv_.reset(); lifecycle_operation_ = false; } ~DeviceContainer() override { shutdown(); } /** * @brief Executes the initialisation * * This will read nodes parameters and initialize * the configuration defined in the parameters. It * will also start loading the components. * * @return true * @return false */ void init(); /** * @brief Executes the initialisation * * Same as init() only that ros parameters are not used. * * @param can_interface_name * @param master_config * @param bus_config * @param master_bin */ void init( const std::string & can_interface_name, const std::string & master_config, const std::string & bus_config, const std::string & master_bin = ""); virtual void configure(); /** * @brief Loads drivers from configuration * * @return true * @return false */ virtual bool load_drivers(); /** * @brief Loads master from configuration * * @return true * @return false */ virtual bool load_master(); /** * @brief Loads the device manager * * @return true * @return false */ virtual bool load_manager(); /** * @brief Load a component * * @param [in] package_name Name of the package * @param [in] driver_name Name of the driver class to load * @param [in] node_id CANopen node id of the target device * @param [in] node_name Node name of the ROS node * @return true * @return false */ virtual bool load_component( const std::string package_name, const std::string driver_name, const uint16_t node_id, const std::string node_name, std::vector & params, const std::string node_namespace = ""); /** * @brief Shutdown all devices. * * This function will shutdown all devices that were created. * The function calls the shutdown function of each created * device. * */ virtual void shutdown() { for (auto it = registered_drivers_.begin(); it != registered_drivers_.end(); ++it) { try { it->second->shutdown(); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } try { can_master_->shutdown(); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } /** * @brief Callback for the listing service * * @param request_header * @param request * @param response */ virtual void on_list_nodes( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) override; /** * @brief Get the registered drivers object * * This function will return a map of all driver objects that were created. * The returned map can be queried by node id. * * @return std::map> */ virtual std::map> get_registered_drivers() { return registered_drivers_; } /** * @brief Get the number of registered drivers * * @return size_t */ virtual size_t count_drivers() { return registered_drivers_.size(); } /** * @brief Get node ids of all drivers with type * * This function gets the ids of all drivers that have the passed * in type. The type must be the fully qualified class name of the * driver as string. * * @param [in] type * @return std::vector */ std::vector get_ids_of_drivers_with_type(std::string type) { std::vector devices; std::vector ids; uint32_t count = this->config_->get_all_devices(devices); if (count == 0) { return ids; } for (auto it = devices.begin(); it != devices.end(); it++) { auto driver_name = config_->get_entry(*it, "driver"); if (driver_name.has_value()) { std::string name = driver_name.value(); if (name.compare(type) == 0) { auto node_id = config_->get_entry(*it, "node_id"); ids.push_back(node_id.value()); } } } return ids; } /** * @brief Get the type of driver by node id * * This function will return the type of the driver that is registered * for the passed node ids. * * @param [in] id * @return std::string */ std::string get_driver_type(uint16_t id) { std::vector devices; uint32_t count = this->config_->get_all_devices(devices); if (count == 0) { return ""; } for (auto it = devices.begin(); it != devices.end(); it++) { auto node_id = config_->get_entry(*it, "node_id"); if (node_id.has_value() && node_id.value() == id) { auto driver_name = config_->get_entry(*it, "driver"); return driver_name.value(); } } } protected: // Components std::map> registered_drivers_; ///< Map of drivers registered in busconfiguration. Name is key. std::shared_ptr can_master_; ///< Pointer to can master instance uint16_t can_master_id_; std::unique_ptr lifecycle_manager_; // Configuration std::shared_ptr config_; ///< Pointer to configuration manager instance std::string dcf_txt_; ///< Cached value of .dcf file parameter std::string bus_config_; ///< Cached value of bus.yml file parameter std::string dcf_bin_; ///< Cached value of .bin file parameter std::string can_interface_name_; ///< Cached value of can interface name bool lifecycle_operation_; // ROS Objects std::weak_ptr executor_; ///< Pointer to ros executor instance rclcpp::Service::SharedPtr init_driver_service_; ///< Service object for init_driver service rclcpp::CallbackGroup::SharedPtr client_cbg_; ///< Callback group for services /** * @brief Set the ROS executor object * * @param [in] executor Pointer to the Executor */ void set_executor(const std::weak_ptr executor); /** * @brief Initialises a driver * * This function needs to be called before a driver can be added * to the CANopen Master Event Loop. It sets the master and executor, * the driver will be added to. * * @param node_id CANopen node id of the target device * * @return true * @return false */ bool init_driver(uint16_t node_id); /** * @brief Callback for init driver service * * @param request * @param response */ void on_init_driver( const canopen_interfaces::srv::CONode::Request::SharedPtr request, canopen_interfaces::srv::CONode::Response::SharedPtr response) { response->success = init_driver(request->nodeid); } /** * @brief Returns a list of components * * @return std::map */ std::map list_components(); /** * @brief Add node to executor * * @param [in] node_interface NodeBaseInterface of the node that should be added to the executor. */ virtual void add_node_to_executor( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface) { if (auto exec = executor_.lock()) { RCLCPP_INFO( this->get_logger(), "Added %s to executor", node_interface->get_fully_qualified_name()); exec->add_node(node_interface, true); } else { RCLCPP_ERROR( this->get_logger(), "Failed to add component %s", node_interface->get_fully_qualified_name()); } } }; } // namespace ros2_canopen #endif // LIFECYCLE_DEVICE_CONTAINER_NODE_HPP ================================================ FILE: canopen_core/include/canopen_core/device_container_error.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef DEVICE_CONTAINER_ERROR_HPP_ #define DEVICE_CONTAINER_ERROR_HPP_ #include #include namespace ros2_canopen { /** * @brief Device Container Exception * * This exception is used, when the device container * fails. * */ class DeviceContainerException : public std::exception { private: std::string what_; public: DeviceContainerException(std::string what) { what_ = what; } char * what(); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/driver_error.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef DRIVER_ERROR_HPP_ #define DRIVER_ERROR_HPP_ #include #include namespace ros2_canopen { /** * @brief Driver Exception * * This exception is used, when a driver * fails. * */ class DriverException : public std::exception { private: std::string what_; public: DriverException(std::string what) { what_ = what; } char * what(); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/driver_node.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef DRIVER_NODE_HPP_ #define DRIVER_NODE_HPP_ #include #include "canopen_core/node_interfaces/node_canopen_driver.hpp" namespace ros2_canopen { /** * @brief Canopen Driver Interface * * This provides an interface that all driver nodes that are loaded * by ros2_canopen::DeviceContainer need to implement. * */ class CanopenDriverInterface { public: /** * @brief Initialise the driver * * This function will initialise the drivers functionalities. * It will be called by the device_container when the node has * been added to the executor. */ virtual void init() = 0; /** * @brief Set the master object * * This function will set the Canopen Master Objects that are * necessary for the driver to be instantiated. It will be called * by the device container when the init_driver service is invoked. * * @param exec * @param master */ virtual void set_master( std::shared_ptr exec, std::shared_ptr master) = 0; /** * @brief Get the node base interface object * * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. * The pointer will be used to add the driver node to the executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() = 0; /** * @brief Shutdown the driver * * This function will shutdown the driver and will especially * join all threads to enable a clean shutdown. * */ virtual void shutdown() = 0; /** * @brief Check whether this is a LifecycleNode * * This function provides runtime information on whether the driver * is a lifecycle driver or not. * * @return true * @return false */ virtual bool is_lifecycle() = 0; /** * @brief Get the node canopen driver interface object * * This function gives access to the underlying NodeCanopenDriverInterface. * * @return std::shared_ptr */ virtual std::shared_ptr get_node_canopen_driver_interface() = 0; }; /** * @brief Canopen Driver * * This provides a class, that driver nodes that are based on rclcpp::Node * should be derived of. This class implements the ros2_canopen::CanopenDriverInterface. * */ class CanopenDriver : public CanopenDriverInterface, public rclcpp::Node { public: std::shared_ptr node_canopen_driver_; explicit CanopenDriver(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp::Node("canopen_driver", node_options) { node_canopen_driver_ = std::make_shared>(this); } /** * @brief Initialise the driver * * This function will activate the driver using the instantiated * node_canopen_driver_. It will call the init, configure, demand_set_master and activate * of the NodeCanopenDriverInterface. If the function finishes without exception, * the driver is activated and ready for use. * */ virtual void init() override; /** * @brief Set the master object * * This function will set the Canopen Master Objects that are * necessary for the driver to be instantiated. It will be called * by the device container when the init_driver service is invoked. * * @param exec * @param master */ virtual void set_master( std::shared_ptr exec, std::shared_ptr master) override; /** * @brief Get the node base interface object * * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. * The pointer will be used to add the driver node to the executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override { return rclcpp::Node::get_node_base_interface(); } /** * @brief Shutdown the driver * * This function will shutdown the driver by calling the * shutdown() function of node_canopen_driver_. * */ virtual void shutdown() override; /** * @brief Check whether this is a LifecycleNode * * @return false */ virtual bool is_lifecycle() override { return false; } /** * @brief Get the node canopen driver interface object * * This function gives access to the underlying NodeCanopenDriverInterface. * * @return std::shared_ptr */ virtual std::shared_ptr get_node_canopen_driver_interface() { return node_canopen_driver_; } }; /** * @brief Lifecycle Canopen Driver * * This provides a class, that driver nodes that are based on rclcpp_lifecycle::LifecycleNode * should be derived of. This class implements the ros2_canopen::CanopenDriverInterface. * */ class LifecycleCanopenDriver : public CanopenDriverInterface, public rclcpp_lifecycle::LifecycleNode { protected: std::shared_ptr node_canopen_driver_; public: explicit LifecycleCanopenDriver(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp_lifecycle::LifecycleNode("lifecycle_canopen_driver", node_options) { node_canopen_driver_ = std::make_shared>(this); } /** * @brief Initialise the driver * * This function will activate the driver using the instantiated * node_canopen_driver_. It will call init() * of the NodeCanopenDriverInterface. If the function finishes without exception, * the driver can be configured. * */ virtual void init() override; /** * @brief Set the master object * * This function will set the Canopen Master Objects that are * necessary for the driver to be instantiated. It will be called * by the device container when the init_driver service is invoked. * * @param exec * @param master */ virtual void set_master( std::shared_ptr exec, std::shared_ptr master) override; /** * @brief Configure Callback * * This function will call configure() and demand_set_master() of the * node_canopen_driver_. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & state); /** * @brief Activate Callback * * This function will call activate() of the * node_canopen_driver_. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & state); /** * @brief Deactivate Callback * * This function will call deactivate() of the * node_canopen_driver_. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state); /** * @brief Deactivate Callback * * This function will call cleanup() of the * node_canopen_driver_. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state); /** * @brief Deactivate Callback * * This function will call shutdown() of the * node_canopen_driver_. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state); /** * @brief Get the node base interface object * * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. * The pointer will be used to add the driver node to the executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override { return rclcpp_lifecycle::LifecycleNode::get_node_base_interface(); } /** * @brief Shutdown the driver * * This function will shutdown the driver by calling the * shutdown() function of node_canopen_driver_. * */ virtual void shutdown() override; /** * @brief Check whether this is a LifecycleNode * * @return true */ virtual bool is_lifecycle() override { return true; } /** * @brief Get the node canopen driver interface object * * This function gives access to the underlying NodeCanopenDriverInterface. * * @return std::shared_ptr */ virtual std::shared_ptr get_node_canopen_driver_interface() { return node_canopen_driver_; } }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/exchange.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef EXCHANGE_HPP #define EXCHANGE_HPP #include #include #include namespace ros2_canopen { struct COData { public: uint16_t index_; uint8_t subindex_; uint32_t data_; }; struct COEmcy { public: uint16_t eec; uint8_t er; uint8_t msef[5]; }; /** * @brief Thread Safe Queue for CANOpen Data Exchange * @tparam T Type of the data to be exchanged * @details This class is a wrapper around boost::lockfree::queue to provide thread safe data * exchange between threads using the queue. */ template class SafeQueue { private: std::size_t capacity_; std::unique_ptr> queue_; public: /** * @brief Constructor for the SafeQueue * @param capacity Capacity of the queue */ explicit SafeQueue(std::size_t capacity = 10) : capacity_(capacity), queue_(new boost::lockfree::queue(capacity_)) { } /** * @brief Push a value to the queue * @param value Value to be pushed */ void push(T value) { queue_->push(std::move(value)); } /** * @brief Try to pop a value from the queue * @return Value if available, boost::none otherwise */ std::optional try_pop() { T value; if (queue_->pop(value)) return std::optional(std::move(value)); return std::optional(); } /** * @brief Try to pop a value from the queue * @param value Value to be returned */ bool try_pop(T & value) { if (queue_->pop(value)) return true; return false; } /** * @brief Wait for a value to be available in the queue * @return Value if available, boost::none otherwise */ boost::optional wait_and_pop() { T value; while (!queue_->pop(value)) boost::this_thread::yield(); return value; } /** * @brief Wait for a value to be available in the queue for a given timeout * @param value Value to be returned */ void wait_and_pop(T & value) { while (!queue_->pop(value)) boost::this_thread::yield(); } /** * @brief Wait for a value to be available in the queue for a given timeout * @param timeout Timeout in milliseconds * @return Value if available, boost::none otherwise */ boost::optional wait_and_pop_for(const std::chrono::milliseconds & timeout) { T value; auto start_time = std::chrono::steady_clock::now(); while (!queue_->pop(value)) { if ( timeout != std::chrono::milliseconds::zero() && std::chrono::steady_clock::now() - start_time >= timeout) return boost::none; boost::this_thread::yield(); } return value; } /** * @brief Wait for a value to be available in the queue for a given timeout * @param timeout Timeout in milliseconds * @param value Value to be returned */ bool wait_and_pop_for(const std::chrono::milliseconds & timeout, T & value) { auto start_time = std::chrono::steady_clock::now(); while (!queue_->pop(value)) { if ( timeout != std::chrono::milliseconds::zero() && std::chrono::steady_clock::now() - start_time >= timeout) return false; boost::this_thread::yield(); } return true; } bool empty() const { return queue_->empty(); } }; } // namespace ros2_canopen #endif // EXCHANGE_HPP ================================================ FILE: canopen_core/include/canopen_core/lifecycle_manager.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef LIFECYCLE_DEVICE_MANAGER_NODE_HPP #define LIFECYCLE_DEVICE_MANAGER_NODE_HPP #include #include #include #include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" //#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "canopen_core/configuration_manager.hpp" #include "canopen_interfaces/srv/co_node.hpp" using namespace std::chrono_literals; /* */ namespace ros2_canopen { /** * @brief Lifecycle Device Manager Node * * This class provides functionalities to coordinate the lifecycle * of master and device drivers that are loaded into the executor. * * Start-up sequence * -------------------------------------------------------------------- * master | unconfigured | x | * | inactive | | x | * | active | | x | x | x | x | x |... * -------------------------------------------------------------------- * drv1 | unconfigured | x | x | x | * | inactive | | x | * | active | | x | x | x |... * -------------------------------------------------------------------- * driv2 | unconfigured | x | x | x | x | x | * | inactive | | x | * | active | | x |... * -------------------------------------------------------------------- * * * */ class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { public: /** * @brief Construct a new Lifecycle Device Manager Node object * * @param node_options */ LifecycleManager(const rclcpp::NodeOptions & node_options) : rclcpp_lifecycle::LifecycleNode("lifecycle_manager", node_options) { this->declare_parameter("container_name", ""); cbg_clients = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); } void init(std::shared_ptr config); protected: rclcpp::CallbackGroup::SharedPtr cbg_clients; std::shared_ptr config_; /** * @brief Callback for the Configure Transition * * This will cause the device manager to load the configuration * from the configuration file. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & state); /** * @brief Callback for the Activate Transition * * This will bringup master and device drivers using the * bring_up_all function. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & state); /** * @brief Callback for the Deactivate Transition * * This will bring down master and device drivers using the * bring_down_all function. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state); /** * @brief Callback for the Cleanup Transition * * Does nothing. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state); /** * @brief Callback for the Shutdown Transition * * Does nothing. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state); std::map::SharedPtr> drivers_get_state_clients; ///< stores node_id and get_state client std::map::SharedPtr> drivers_change_state_clients; ///< stores node_id and change_state client std::map device_names_to_ids; // stores device_name and node_id /** * @todo Remove relicts? * */ rclcpp::Client::SharedPtr add_driver_client_; ///< Service client object for adding a driver rclcpp::Client::SharedPtr remove_driver_client_; ///< Service client object for removing a driver uint8_t master_id_; ///< Stores master id std::string container_name_; ///< Stores name of the associated device_container protected: template std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) { auto end = std::chrono::steady_clock::now() + time_to_wait; std::chrono::milliseconds wait_period(100); std::future_status status = std::future_status::timeout; do { auto now = std::chrono::steady_clock::now(); auto time_left = end - now; if (time_left <= std::chrono::seconds(0)) { break; } status = future.wait_for((time_left < wait_period) ? time_left : wait_period); } while (rclcpp::ok() && status != std::future_status::ready); return status; } /** * @brief Get the lifecycle state of a driver node * * @param [in] node_id * @param [in] time_out * @return unsigned int */ virtual unsigned int get_state(uint8_t node_id, std::chrono::seconds time_out); /** * @brief Change the lifecycle state of a driver node * * @param [in] node_id CANopen node id of the driver * @param [in] transition Lifecycle transition to trigger * @param [in] time_out Timeout * @return true * @return false */ virtual bool change_state(uint8_t node_id, uint8_t transition, std::chrono::seconds time_out); /** * @brief Brings up master and all drivers * * This funnnction brings up the CANopen master and all drivers that are defined * in the configuration. The order of bringing up drivers is arbitrary. * * @return true * @return false */ virtual bool bring_up_all(); /** * @brief Brings down master and all drivers * * This funnnction brings up the CANopen master and all drivers that are defined * in the configuration. First all drivers are brought down, then the master. * * @return true * @return false */ virtual bool bring_down_all(); /** * @brief Brings up master * * This function brings up the CANopen master. The master transitioned twice, * first the configure transition is triggered. Once the transition is successfully * finished, the activate transition is triggered. * * @return true * @return false */ virtual bool bring_up_master(); /** * @brief Bring down master * * This function brrignsdown the CANopen master. The master transitioned twice, * first the deactivate transition is triggered. Once the transition is successfully * finished, the cleanup transition is triggered. * * @return true * @return false */ virtual bool bring_down_master(); /** * @brief Brings up the drivers with specified node_id * * This function bringsup the CANopen driver for the device with the specified * node_id. The driver is transitioned twice, * first the configure transition is triggered. Once the transition is successfully * finished, the activate transition is triggered. * * @param device_name * @return true * @return false */ virtual bool bring_up_driver(std::string device_name); /** * @brief Brings down the driver with specified node_id * * This function brings down the CANopen driver for the device with the specified * node_id. The driver is transitioned twice, * first the deactivate transition is triggered. Once the transition is successfully * finished, the cleanup transition is triggered. * * @param device_name * @return true * @return false */ virtual bool bring_down_driver(std::string device_name); /** * @brief Load information from configuration * * This function loads the information about the CANopen Bus specified in * the configuration file and creates the necessary ROS2 services and clients. * * @return true * @return false */ virtual bool load_from_config(); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/master_error.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef MASTER_ERROR_HPP_ #define MASTER_ERROR_HPP_ #include namespace ros2_canopen { /** * @brief Master Exception * * This exception is used, when a master * fails. * */ class MasterException : public std::exception { private: std::string what_; public: MasterException(std::string what) { what_ = what; } char * what(); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/master_node.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef MASTER_NODE_HPP_ #define MASTER_NODE_HPP_ #include #include "canopen_core/node_interfaces/node_canopen_master.hpp" namespace ros2_canopen { /** * @brief Canopen Master Interface * * This class provides the interface that all master's that are loaded * by the device container need to implement. */ class CanopenMasterInterface { public: /** * @brief Initialise the master * * This function should initialise the master's functionality. * It will be called by the device container after the node was * added to the ros executor. */ virtual void init() = 0; /** * @brief Shutdown the driver * * This function should shutdown all functionality and make sure that * the master will exit cleanly. It will be called by the device container * before exiting. * */ virtual void shutdown() = 0; /** * @brief Get the master object * * This function should return the lely master object. It should * throw a ros2_canopen::MasterException if the master object * was not yet set. * * @return std::shared_ptr */ virtual std::shared_ptr get_master() = 0; /** * @brief Get the executor object * * This function gets the lely executor object. It should throw * a ros2_canopen::MasterException if the executor object is not * yet instantiated. * * @return std::shared_ptr */ virtual std::shared_ptr get_executor() = 0; /** * @brief Get the node base interface object * * This function should return the NodeBaseInterface of the associated * ROS node. This is used by device container to add the master to the * ros executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() = 0; /** * @brief Check whether the node is a lifecycle node * * @return true * @return false */ virtual bool is_lifecycle() = 0; }; /** * @brief Canopen Master * * This class implements the Canopen Master Interface and is derived * from rclcpp::Node. All unmanaged master nodes should inherit from * this class. * */ class CanopenMaster : public CanopenMasterInterface, public rclcpp::Node { public: std::shared_ptr node_canopen_master_; explicit CanopenMaster(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp::Node("canopen_master", node_options) { // node_canopen_master_ = // std::make_shared>(this); } /** * @brief Initialises the driver * * This function initialises the driver. It is called by the * device container after adding the node to the ros executor. * This function uses the NodeCanopenMasterInterface. It will * call init(), configure() and activate(). If this function * does not throw, the driver is successfully initialised and * ready to be used. * */ virtual void init() override; /** * @brief Shuts the master down * * This function is called by the device container before exiting, * it should enable a clean exit of the master and especially join * all threads. It will call the shutdown() function of the * NodeCanopenMasterInterface object associated with this class. */ virtual void shutdown() override; /** * @brief Get the master object * * This function should return the lely master object. It should * throw a ros2_canopen::MasterException if the master object * was not yet set. * * @return std::shared_ptr */ virtual std::shared_ptr get_master() override; /** * @brief Get the executor object * * This function gets the lely executor object. It should throw * a ros2_canopen::MasterException if the executor object is not * yet instantiated. * * @return std::shared_ptr */ virtual std::shared_ptr get_executor() override; /** * @brief Get the node base interface object * * This function should return the NodeBaseInterface of the associated * ROS node. This is used by device container to add the master to the * ros executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override { return rclcpp::Node::get_node_base_interface(); } /** * @brief Check whether the node is a lifecycle node * * @return false */ virtual bool is_lifecycle() { return false; } }; class LifecycleCanopenMaster : public CanopenMasterInterface, public rclcpp_lifecycle::LifecycleNode { protected: std::shared_ptr node_canopen_master_; public: LifecycleCanopenMaster(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp_lifecycle::LifecycleNode("lifecycle_canopen_master", node_options) { node_canopen_master_ = std::make_shared>(this); } /** * @brief Initialises the driver * * This function initialises the driver. It is called by the * device container after adding the node to the ros executor. * This function uses the NodeCanopenMasterInterface. It will * call init(). If this function * does not throw, the driver is successfully initialised and * ready to switch through the lifecycle. * */ virtual void init() override; /** * @brief Shuts the master down * * This function is called by the device container before exiting, * it should enable a clean exit of the master and especially join * all threads. It will call the shutdown() function of the * NodeCanopenMasterInterface object associated with this class. */ virtual void shutdown() override; /** * @brief Get the master object * * This function should return the lely master object. It should * throw a ros2_canopen::MasterException if the master object * was not yet set. * * @return std::shared_ptr */ virtual std::shared_ptr get_master() override; /** * @brief Get the executor object * * This function gets the lely executor object. It should throw * a ros2_canopen::MasterException if the executor object is not * yet instantiated. * * @return std::shared_ptr */ virtual std::shared_ptr get_executor() override; /** * @brief Configure Transition Callback * * This function will call the configure() function of the * NodeCanopenMasterInterface object associated with this class. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & state); /** * @brief Activate Transition Callback * * This function will call the active() function of the * NodeCanopenMasterInterface object associated with this class. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & state); /** * @brief Deactivet Transition Callback * * This function will call the deactivate() function of the * NodeCanopenMasterInterface object associated with this class. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state); /** * @brief Cleanup Transition Callback * * This function will call the cleanup() function of the * NodeCanopenMasterInterface object associated with this class. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state); /** * @brief Shutdown Transition Callback * * This function will call the shutdown() function of the * NodeCanopenMasterInterface object associated with this class. * * @param state * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn */ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state); /** * @brief Get the node base interface object * * This function should return the NodeBaseInterface of the associated * ROS node. This is used by device container to add the master to the * ros executor. * * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr */ virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override { return rclcpp_lifecycle::LifecycleNode::get_node_base_interface(); } /** * @brief Check whether the node is a lifecycle node * * @return true */ virtual bool is_lifecycle() { return true; } }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_DRIVER_HPP_ #define NODE_CANOPEN_DRIVER_HPP_ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "canopen_core/driver_error.hpp" #include "canopen_core/node_interfaces/node_canopen_driver_interface.hpp" #include "canopen_core/visibility_control.h" #include "canopen_interfaces/srv/co_node.hpp" using namespace rclcpp; namespace ros2_canopen { namespace node_interfaces { /** * @brief Node Canopen Driver * * This class implements the NodeCanopenDriverInterface. It provides * core functionality and logic for CanopenDriver, indepentently of the * ROS node type. Currently rclcpp::Node and rclcpp_lifecycle::LifecycleNode * and derived classes are supported. Other node types will lead to compile * time error. * * @tparam NODETYPE */ template class NodeCanopenDriver : public NodeCanopenDriverInterface { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: NODETYPE * node_; std::shared_ptr exec_; std::shared_ptr master_; std::shared_ptr driver_; std::chrono::milliseconds non_transmit_timeout_; YAML::Node config_; uint8_t node_id_; std::string container_name_; std::string eds_; std::string bin_; rclcpp::CallbackGroup::SharedPtr client_cbg_; rclcpp::CallbackGroup::SharedPtr timer_cbg_; std::atomic master_set_; std::atomic initialised_; std::atomic configured_; std::atomic activated_; public: NodeCanopenDriver(NODETYPE * node) : master_set_(false), initialised_(false), configured_(false), activated_(false) { node_ = node; } /** * @brief Set Master * * Sets the Lely Canopen Master Objects needed by the driver * to add itself to the master. * */ virtual void set_master( std::shared_ptr exec, std::shared_ptr master) { RCLCPP_DEBUG(node_->get_logger(), "set_master_start"); if (!configured_.load()) { throw DriverException("Set Master: driver is not configured"); } if (activated_.load()) { throw DriverException("Set Master: driver is not activated"); } this->exec_ = exec; this->master_ = master; this->master_set_.store(true); RCLCPP_DEBUG(node_->get_logger(), "set_master_end"); } /** * @brief Initialise the driver * * In this function the ROS interface should be created and * potentially necessary callback groups. * */ void init() { RCLCPP_DEBUG(node_->get_logger(), "init_start"); if (configured_.load()) { throw DriverException("Init: Driver is already configured"); } if (activated_.load()) { throw DriverException("Init: Driver is already activated"); } client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); node_->declare_parameter("container_name", ""); node_->declare_parameter("node_id", 0); node_->declare_parameter("non_transmit_timeout", 100); node_->declare_parameter("config", ""); this->init(true); this->initialised_.store(true); RCLCPP_DEBUG(node_->get_logger(), "init_end"); } /** * @brief Initialises the driver * * This does not do anything, it is an empty function. it * should be overridden by derived classes. * * @todo * Potentially make this an abstract function. This is mainly * for debugging purposes. * * @param called_from_base */ virtual void init(bool called_from_base) {} /** * @brief Configure the driver * * This function should contain the configuration related things, * such as reading parameter data or configuration data from files. * * This function reads the parameters container_name, non_transmit_timeout, * node_id and config. Once done it will call the configure(bool) function * that should be over * */ void configure() { RCLCPP_DEBUG(node_->get_logger(), "configure_start"); if (!initialised_.load()) { throw DriverException("Configure: driver is not initialised"); } if (configured_.load()) { throw DriverException("Configure: driver is already configured"); } if (activated_.load()) { throw DriverException("Configure: driver is already activated"); } int non_transmit_timeout; std::string config; node_->get_parameter("container_name", container_name_); node_->get_parameter("non_transmit_timeout", non_transmit_timeout); node_->get_parameter("node_id", this->node_id_); node_->get_parameter("config", config); this->config_ = YAML::Load(config); this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout); auto path = this->config_["dcf_path"].template as(); auto dcf = this->config_["dcf"].template as(); auto name = this->node_->get_name(); eds_ = path + "/" + dcf; bin_ = path + "/" + name + ".bin"; this->configure(true); this->configured_.store(true); RCLCPP_DEBUG(node_->get_logger(), "configure_end"); } /** * @brief Configure the driver * * This function should be overridden by derived classes. * * @param called_from_base */ virtual void configure(bool called_from_base) {} /** * @brief Activate the driver * * This function should activate the driver, consequently it needs to start all timers and threads * necessary for the operation of the driver. * */ void activate() { RCLCPP_DEBUG(node_->get_logger(), "activate_start"); if (!master_set_.load()) { throw DriverException("Activate: master is not set"); } if (!initialised_.load()) { throw DriverException("Activate: driver is not initialised"); } if (!configured_.load()) { throw DriverException("Activate: driver is not configured"); } if (activated_.load()) { throw DriverException("Activate: driver is already activated"); } this->add_to_master(); this->activate(true); this->activated_.store(true); RCLCPP_DEBUG(node_->get_logger(), "activate_end"); } /** * @brief Activates the driver. * * This function should be overridden by derived * classes. * * @param called_from_base */ virtual void activate(bool called_from_base) {} /** * @brief Deactivate the driver * * This function should deactivate the driver, consequently it needs to stop all timers and * threads that are related to the operation of the diver. * */ void deactivate() { RCLCPP_DEBUG(node_->get_logger(), "deactivate_start"); if (!master_set_.load()) { throw DriverException("Activate: master is not set"); } if (!initialised_.load()) { throw DriverException("Deactivate: driver is not initialised"); } if (!configured_.load()) { throw DriverException("Deactivate: driver is not configured"); } if (!activated_.load()) { throw DriverException("Deactivate: driver is not activated"); } this->activated_.store(false); this->remove_from_master(); this->deactivate(true); RCLCPP_DEBUG(node_->get_logger(), "deactivate_end"); } /** * @brief Deactivates the driver * * This function should be overridden by derived classes. * * @param called_from_base */ virtual void deactivate(bool called_from_base) {} /** * @brief Cleanup the driver * * This function needs to clean the internal state of the driver. This means * all data should be deleted. * */ void cleanup() { if (!initialised_.load()) { throw DriverException("Cleanup: driver is not initialised"); } if (!configured_.load()) { throw DriverException("Cleanup: driver is not configured"); } if (activated_.load()) { throw DriverException("Cleanup: driver is still activated"); } this->cleanup(true); this->configured_.store(false); } /** * @brief Cleanup the driver * * This function should be overridden by derived classes. * * @param called_from_base */ virtual void cleanup(bool called_from_base) { RCLCPP_INFO(this->node_->get_logger(), "Cleanup"); this->exec_.reset(); this->master_.reset(); this->master_set_.store(false); } /** * @brief Shutdown the driver * * This function should shutdown the driver. * */ void shutdown() { RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down."); if (this->activated_) { this->deactivate(); } if (this->configured_) { this->cleanup(); } shutdown(true); this->master_set_.store(false); this->initialised_.store(false); this->configured_.store(false); this->activated_.store(false); } /** * @brief Shuts down the driver * * This function should be overridden by derived classes. * * @param called_from_base */ virtual void shutdown(bool called_from_base) {} virtual void demand_set_master(); protected: /** * @brief Add the driver to master * */ virtual void add_to_master() { throw DriverException("Add to master not implemented."); } /** * @brief Remove the driver from master * */ virtual void remove_from_master() { throw DriverException("Remove from master not implemented."); } }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_DRIVER_INTERFACE_HPP_ #define NODE_CANOPEN_DRIVER_INTERFACE_HPP_ #include #include namespace ros2_canopen { namespace node_interfaces { /** * @brief Node Canopen Driver Interface * * This node provides the interface for NodeCanopenDriver classes * that provide ROS node independent CANopen functionality. * */ class NodeCanopenDriverInterface { public: NodeCanopenDriverInterface() {} /** * @brief Set Master * * Sets the Lely Canopen Master Objects needed by the driver * to add itself to the master. * */ virtual void set_master( std::shared_ptr exec, std::shared_ptr master) = 0; /** * @brief Demand set Master * * This function should ask the drivers parent container to * set the master objects. This should result in a call to * set_master function. * */ virtual void demand_set_master() = 0; /** * @brief Initialise the driver * * In this function the ROS interface should be created and * potentially necessary callback groups. * */ virtual void init() = 0; /** * @brief Configure the driver * * This function should contain the configuration related things, * such as reading parameter data or configuration data from files. * */ virtual void configure() = 0; /** * @brief Activate the driver * * This function should activate the driver, consequently it needs to start all timers and threads * necessary for the operation of the driver. * */ virtual void activate() = 0; /** * @brief Deactivate the driver * * This function should deactivate the driver, consequently it needs to stop all timers and * threads that are related to the operation of the diver. * */ virtual void deactivate() = 0; /** * @brief Cleanup the driver * * This function needs to clean the internal state of the driver. This means * all data should be deleted. * */ virtual void cleanup() = 0; /** * @brief Shutdown the driver * * This function should shutdown the driver. * */ virtual void shutdown() = 0; /** * @brief Add the driver to master * */ virtual void add_to_master() = 0; /** * @brief Remove the driver from master * */ virtual void remove_from_master() = 0; }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_MASTER_HPP_ #define NODE_CANOPEN_MASTER_HPP_ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "canopen_core/master_error.hpp" #include "canopen_core/node_interfaces/node_canopen_master_interface.hpp" namespace ros2_canopen { namespace node_interfaces { /** * @brief Node Canopen Master * * This class implements the NodeCanopenMasterInterface. It provides * core functionality and logic for CanopenMaster, indepentently of the * ROS node type. Currently rclcpp::Node and rclcpp_lifecycle::LifecycleNode * and derived classes are supported. Other node types will lead to compile * time error. * * @tparam NODETYPE */ template class NodeCanopenMaster : public NodeCanopenMasterInterface { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: NODETYPE * node_; std::atomic initialised_; std::atomic configured_; std::atomic activated_; std::atomic master_set_; std::shared_ptr master_; std::shared_ptr exec_; std::unique_ptr io_guard_; std::unique_ptr ctx_; std::unique_ptr poll_; std::unique_ptr loop_; std::unique_ptr timer_; std::unique_ptr ctrl_; std::unique_ptr chan_; std::unique_ptr sigset_; rclcpp::CallbackGroup::SharedPtr client_cbg_; rclcpp::CallbackGroup::SharedPtr timer_cbg_; YAML::Node config_; uint8_t node_id_; std::chrono::milliseconds non_transmit_timeout_; std::string container_name_; std::string master_dcf_; std::string master_bin_; std::string can_interface_name_; uint32_t timeout_; std::thread spinner_; public: NodeCanopenMaster(NODETYPE * node) : initialised_(false), configured_(false), activated_(false), master_set_(false) { node_ = node; } /** * @brief Initialise Master * */ void init() override { RCLCPP_DEBUG(node_->get_logger(), "init_start"); if (configured_.load()) { throw MasterException("Init: Master is already configured."); } if (activated_.load()) { throw MasterException("Init: Master is already activated."); } client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); node_->declare_parameter("container_name", ""); node_->declare_parameter("master_dcf", ""); node_->declare_parameter("master_bin", ""); node_->declare_parameter("can_interface_name", "vcan0"); node_->declare_parameter("node_id", 0); node_->declare_parameter("non_transmit_timeout", 100); node_->declare_parameter("config", ""); this->init(true); this->initialised_.store(true); RCLCPP_DEBUG(node_->get_logger(), "init_end"); } virtual void init(bool called_from_base) {} /** * @brief Configure the driver * * This function should contain the configuration related things, * such as reading parameter data or configuration data from files. * */ void configure() override { if (!initialised_.load()) { throw MasterException("Configure: Master is not initialised."); } if (configured_.load()) { throw MasterException("Configure: Master is already configured."); } if (activated_.load()) { throw MasterException("Configure: Master is already activated."); } int non_transmit_timeout; std::string config; node_->get_parameter("container_name", container_name_); node_->get_parameter("master_dcf", master_dcf_); node_->get_parameter("master_bin", master_bin_); node_->get_parameter("can_interface_name", can_interface_name_); node_->get_parameter("node_id", node_id_); node_->get_parameter("non_transmit_timeout", non_transmit_timeout); node_->get_parameter("config", config); this->config_ = YAML::Load(config); this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout); this->configure(true); this->configured_.store(true); } virtual void configure(bool called_from_base) { std::optional timeout; try { timeout = this->config_["boot_timeout"].as(); } catch (...) { RCLCPP_WARN( this->node_->get_logger(), "No timeout parameter found in config file. Using default value of 100ms."); } this->timeout_ = timeout.value_or(2000); RCLCPP_INFO_STREAM( this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms."); } /** * @brief Activate the driver * * This function should activate the driver, consequently it needs to start all timers and threads * necessary for the operation of the driver. * */ void activate() override { RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate start"); if (!initialised_.load()) { throw MasterException("Activate: master is not initialised"); } if (!configured_.load()) { throw MasterException("Activate: master is not configured"); } if (activated_.load()) { throw MasterException("Activate: master is already activated"); } io_guard_ = std::make_unique(); ctx_ = std::make_unique(); poll_ = std::make_unique(*ctx_); loop_ = std::make_unique(poll_->get_poll()); exec_ = std::make_shared(loop_->get_executor()); timer_ = std::make_unique(*poll_, *exec_, CLOCK_MONOTONIC); ctrl_ = std::make_unique(can_interface_name_.c_str()); chan_ = std::make_unique(*poll_, *exec_); chan_->open(*ctrl_); this->activate(true); if (!master_) { throw MasterException("Activate: master not set"); } this->master_set_.store(true); this->master_->Reset(); this->spinner_ = std::thread( [this]() { try { loop_->run(); } catch (const std::exception & e) { RCLCPP_INFO(this->node_->get_logger(), e.what()); } RCLCPP_INFO(this->node_->get_logger(), "Canopen master loop stopped"); }); this->activated_.store(true); RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate end"); } /** * @brief Activate hook for derived classes * * This function should create a Master using exec_, timer_, master_dcf_, master_bin_ * and node_id_ members and store it in master_. It should also create a thread and run * the master's event loop. * * @param called_from_base */ virtual void activate(bool called_from_base) {} /** * @brief Deactivate the driver * * This function should deactivate the driver, consequently it needs to stop all timers and * threads that are related to the operation of the diver. * */ void deactivate() override { if (!initialised_.load()) { throw MasterException("Deactivate: master is not initialised"); } if (!configured_.load()) { throw MasterException("Deactivate: master is not configured"); } if (!activated_.load()) { throw MasterException("Deactivate: master is not activated"); } exec_->post( [this]() { RCLCPP_INFO(node_->get_logger(), "Lely Core Context Shutdown"); ctx_->shutdown(); }); this->spinner_.join(); this->deactivate(true); this->activated_.store(false); } /** * @brief Deactivate hook for derived classes * * This function should wait to join the thread created in the activate * function. * * @param called_from_base */ virtual void deactivate(bool called_from_base) {} /** * @brief Cleanup the driver * * This function needs to clean the internal state of the driver. This means * all data should be deleted. * */ void cleanup() override { if (!initialised_.load()) { throw MasterException("Cleanup: master is not initialised."); } if (!configured_.load()) { throw MasterException("Cleanup: master is not configured"); } if (activated_.load()) { throw MasterException("Cleanup: master is still active"); } this->cleanup(true); io_guard_.reset(); ctx_.reset(); poll_.reset(); loop_.reset(); exec_.reset(); timer_.reset(); ctrl_.reset(); chan_.reset(); this->configured_.store(false); this->master_set_.store(false); } virtual void cleanup(bool called_from_base) {} /** * @brief Shutdown the driver * * This function should shutdown the driver. * */ void shutdown() override { RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down."); if (this->activated_) { this->deactivate(); } if (this->configured_) { this->cleanup(); } shutdown(true); this->master_set_.store(false); this->initialised_.store(false); this->configured_.store(false); this->activated_.store(false); } virtual void shutdown(bool called_from_base) {} /** * @brief Get the master object * * @return std::shared_ptr */ virtual std::shared_ptr get_master() { if (!master_set_.load()) { throw MasterException("Get Master: Master is not set."); } return master_; } /** * @brief Get the executor object * * @return std::shared_ptr */ virtual std::shared_ptr get_executor() { if (!master_set_.load()) { throw MasterException("Get Executor: master is not set"); } return exec_; } }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_MASTER_INTERFACE_HPP_ #define NODE_CANOPEN_MASTER_INTERFACE_HPP_ #include #include #include "canopen_core/node_interfaces/node_canopen_driver_interface.hpp" namespace ros2_canopen { namespace node_interfaces { /** * @brief Node Canopen Master Interface * * This node provides the interface for NodeCanopenMaster classes * that provide ROS node independent CANopen functionality. * */ class NodeCanopenMasterInterface { public: /** * @brief Initialise Master * */ virtual void init() = 0; /** * @brief Configure the driver * * This function should contain the configuration related things, * such as reading parameter data or configuration data from files. * */ virtual void configure() = 0; /** * @brief Activate the driver * * This function should activate the driver, consequently it needs to start all timers and threads * necessary for the operation of the driver. * */ virtual void activate() = 0; /** * @brief Deactivate the driver * * This function should deactivate the driver, consequently it needs to stop all timers and * threads that are related to the operation of the diver. * */ virtual void deactivate() = 0; /** * @brief Cleanup the driver * * This function needs to clean the internal state of the driver. This means * all data should be deleted. * */ virtual void cleanup() = 0; /** * @brief Shutdown the driver * * This function should shutdown the driver. * */ virtual void shutdown() = 0; /** * @brief Get the master object * * @return std::shared_ptr */ virtual std::shared_ptr get_master() = 0; /** * @brief Get the executor object * * @return std::shared_ptr */ virtual std::shared_ptr get_executor() = 0; }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_core/include/canopen_core/visibility_control.h ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_CORE__VISIBILITY_CONTROL_H_ #define CANOPEN_CORE__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_CORE_EXPORT __attribute__((dllexport)) #define CANOPEN_CORE_IMPORT __attribute__((dllimport)) #else #define CANOPEN_CORE_EXPORT __declspec(dllexport) #define CANOPEN_CORE_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_CORE_BUILDING_LIBRARY #define CANOPEN_CORE_PUBLIC CANOPEN_CORE_EXPORT #else #define CANOPEN_CORE_PUBLIC CANOPEN_CORE_IMPORT #endif #define CANOPEN_CORE_PUBLIC_TYPE CANOPEN_CORE_PUBLIC #define CANOPEN_CORE_LOCAL #else #define CANOPEN_CORE_EXPORT __attribute__((visibility("default"))) #define CANOPEN_CORE_IMPORT #if __GNUC__ >= 4 #define CANOPEN_CORE_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_CORE_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_CORE_PUBLIC #define CANOPEN_CORE_LOCAL #endif #define CANOPEN_CORE_PUBLIC_TYPE #endif #endif // CANOPEN_CORE__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_core/launch/canopen.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os import sys sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa import launch import launch.actions import launch.events import launch_ros import launch_ros.events import launch_ros.events.lifecycle from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution from launch.actions import DeclareLaunchArgument import lifecycle_msgs.msg def generate_launch_description(): bus_conf_arg = DeclareLaunchArgument( "bus_config", default_value=TextSubstitution(text=""), description="Path to the bus configuration to use.", ) master_conf_arg = DeclareLaunchArgument( "master_config", default_value=TextSubstitution(text=""), description="Path to the master configuration to use (dcf).", ) master_bin_arg = DeclareLaunchArgument( "master_bin", default_value=TextSubstitution(text=""), description="Path to the master configuration to use (bin).", ) can_interface_name_arg = DeclareLaunchArgument( "can_interface_name", default_value=TextSubstitution(text="vcan0"), description="CAN interface used by master and drivers.", ) namespace_arg = DeclareLaunchArgument( "namespace", default_value=TextSubstitution(text=""), description="Namespace for the device container node.", ) ld = launch.LaunchDescription() logging = launch.actions.GroupAction( actions=[ launch.actions.LogInfo(msg=LaunchConfiguration("bus_config")), launch.actions.LogInfo(msg=LaunchConfiguration("master_config")), launch.actions.LogInfo(msg=LaunchConfiguration("master_bin")), launch.actions.LogInfo(msg=LaunchConfiguration("can_interface_name")), ] ) lifecycle_device_container_node = launch_ros.actions.LifecycleNode( name="device_container_node", namespace=LaunchConfiguration("namespace"), package="canopen_core", output="screen", executable="device_container_node", parameters=[ {"bus_config": LaunchConfiguration("bus_config")}, {"master_config": LaunchConfiguration("master_config")}, {"master_bin": LaunchConfiguration("master_bin")}, {"can_interface_name": LaunchConfiguration("can_interface_name")}, ], ) ld.add_action(bus_conf_arg) ld.add_action(master_conf_arg) ld.add_action(master_bin_arg) ld.add_action(can_interface_name_arg) ld.add_action(namespace_arg) ld.add_action(logging) ld.add_action(lifecycle_device_container_node) return ld ================================================ FILE: canopen_core/package.xml ================================================ canopen_core 0.3.2 Core ros2_canopen functionalities such as DeviceContainer and master Christoph Hellmann Santos Apache-2.0 ament_cmake canopen_interfaces lely_core_libraries lifecycle_msgs rclcpp rclcpp_components rclcpp_lifecycle yaml_cpp_vendor boost ament_lint_auto ament_cmake ================================================ FILE: canopen_core/readme.md ================================================ # ROS2 canopen {#mainpage} Welcome to the ROS2 canopen documentation. ## About ROS2 CANopen is being developed and maintainted by the [ROS-Industrial Consortium](rosindustrial.org). The package heavily builds on top of [Lely's canopen stack](https://opensource.lely.com/canopen/). ## Getting started Clone the ros2_canopen repository into your workspace's source directory and build: git clone git@gitlab.cc-asp.fraunhofer.de:ipa326/ros-industrial/ros2_canopen.git cd .. colcon build . install/setup.sh Once built, you are ready to use the package but you need to setup the can interface. ## Setting up your CANopen network configuration In order to use the package for your CANopen network, you need to create a network configuration. We recommend the tools Lely CANopen provides for this (dcfgen). How to do this: 1. Gather the EDS files for the CANopen devices connected to your network and put them into a folder. 2. Create a yaml description of your network. Find an example below. Further options can be found and documentation of the yaml file structure can be found [here](https://opensource.lely.com/canopen/docs/dcf-tools/). ROS2 canopen extends the slave description by the tag driver, which specifies the driver to be used for the specific device. The driver is later loaded using pluginlib. master: node_id: 1 motioncontroller_1: node_id: 2 dcf: "simple.eds" driver: "BasicDevice" 3. Generate a master.dcf file using Lely CANopen's dcfgen tool. This outputs a master.dcf. dcfgen -r [path to yaml file] 4. Create a ROS parameters YAML file setting the path to your created dcf and yaml file: canopen_master: ros__parameters: dcf_path: [Absolute path to your master dcf] yaml_path: [Absolute path to your bus yaml] can_interface_name: [interface name] ## ROS2 canopen (preliminary) 1. ros2 launch bring_up_master.launch.py parameter_file_path:=[Path to your config yaml] 2. Set configuration files and can interface. sh ros2 param set /canopen_master/dcf_path [path to dcf] ros2 param set /canopen_master/yaml_path [path to yaml] ros2 param set /canopen_master/can_interface_name [if_name] 3. Now you can configure the canopen_master via lifecycle management. The master will now load the configuration files and spawn the necessary driver nodes for each device on the network. You can check the new nodes with ros2 node command. ros2 lifecycle set /canopen_master configure ros2 node list 4. Configure all driver nodes with lifecycle management 5. Activate canopen_master node and then all driver nodes. 6. The system should now be running and you should be able to use the driver nodes to communicate with your devices. ================================================ FILE: canopen_core/scripts/setup_vcan.sh ================================================ modprobe vcan ip link add dev vcan0 type vcan ip link set up vcan0 ================================================ FILE: canopen_core/src/configuration_manager.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/configuration_manager.hpp" #include namespace ros2_canopen { void ConfigurationManager::init_config() { std::string dcf_path = ""; for (YAML::const_iterator it = root_.begin(); it != root_.end(); it++) { std::string driver_name = it->first.as(); if (driver_name != "options") continue; YAML::Node config_node = it->second; if (config_node["dcf_path"]) { dcf_path = config_node["dcf_path"].as(); } } for (YAML::const_iterator it = root_.begin(); it != root_.end(); it++) { std::string driver_name = it->first.as(); if (driver_name == "options") continue; YAML::Node config_node = it->second; if (!config_node["dcf_path"]) { config_node["dcf_path"] = dcf_path; } devices_.insert({driver_name, config_node}); } } uint32_t ConfigurationManager::get_all_devices(std::vector & devices) { uint32_t count = 0; for (auto it = devices_.begin(); it != devices_.end(); it++) { devices.emplace_back(it->first); count++; } return count; } } // namespace ros2_canopen ================================================ FILE: canopen_core/src/device_container.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/device_container.hpp" #include "canopen_core/device_container_error.hpp" using namespace ros2_canopen; void DeviceContainer::set_executor(const std::weak_ptr executor) { executor_ = executor; } bool DeviceContainer::init_driver(uint16_t node_id) { RCLCPP_DEBUG(this->get_logger(), "init_driver"); registered_drivers_[node_id]->set_master( this->can_master_->get_executor(), this->can_master_->get_master()); return true; } bool DeviceContainer::load_component( const std::string package_name, const std::string driver_name, const uint16_t node_id, const std::string node_name, std::vector & params, const std::string node_namespace) { ComponentResource component; std::string resource_index("rclcpp_components"); std::vector components = this->get_component_resources(package_name, resource_index); for (auto it = components.begin(); it != components.end(); ++it) { if (it->first.compare(driver_name) == 0) { std::shared_ptr factory_node = this->create_component_factory(*it); rclcpp::NodeOptions opts; opts.use_global_arguments(false); opts.use_intra_process_comms(true); std::vector remap_rules; remap_rules.push_back("--ros-args"); // remap_rules.push_back("--log-level"); // remap_rules.push_back("debug"); remap_rules.push_back("-r"); remap_rules.push_back("__node:=" + node_name); // Prefer node_namespace if provided, else fallback to container namespace std::string canopen_ns = node_namespace.empty() ? this->get_namespace() : node_namespace; remap_rules.push_back("-r"); remap_rules.push_back("__ns:=" + canopen_ns); if (node_namespace.empty()) { RCLCPP_WARN( this->get_logger(), "Namespace not provided for node '%s', using container namespace: %s", node_name.c_str(), canopen_ns.c_str()); } else { RCLCPP_INFO( this->get_logger(), "Namespace set for node '%s': %s", node_name.c_str(), node_namespace.c_str()); } opts.arguments(remap_rules); opts.parameter_overrides(params); try { auto wrapper = factory_node->create_node_instance(opts); if (node_name.compare("master") == 0) { RCLCPP_INFO(this->get_logger(), "Load master component."); can_master_ = std::static_pointer_cast(wrapper.get_node_instance()); } else { RCLCPP_INFO(this->get_logger(), "Load driver component."); if (this->lifecycle_operation_) { auto node = std::static_pointer_cast(wrapper.get_node_instance()); if (!node->is_lifecycle()) { std::string execption_string; execption_string.append("Driver "); execption_string.append(driver_name); execption_string.append(" for device "); execption_string.append(node_name); execption_string.append(" is not a lifecycle driver while the master is."); RCLCPP_ERROR(this->get_logger(), execption_string.c_str()); throw DeviceContainerException(execption_string); } else { registered_drivers_[node_id] = node; } } else { auto node = std::static_pointer_cast(wrapper.get_node_instance()); if (node->is_lifecycle()) { std::string execption_string; execption_string.append("Driver "); execption_string.append(driver_name); execption_string.append(" for device "); execption_string.append(node_name); execption_string.append(" is a lifecycle driver while the master is not."); RCLCPP_ERROR(this->get_logger(), execption_string.c_str()); throw DeviceContainerException(execption_string); } else { registered_drivers_[node_id] = node; } } } } catch (const std::exception & ex) { // In the case that the component constructor throws an exception, // rethrow into the following catch block. throw DeviceContainerException( "Component constructor threw an exception: " + std::string(ex.what())); } catch (...) { // In the case that the component constructor throws an exception, // rethrow into the following catch block. throw DeviceContainerException("Component constructor threw an exception"); } return true; } } return false; } void DeviceContainer::configure() { if (!this->get_parameter("can_interface_name", can_interface_name_)) { RCLCPP_ERROR(this->get_logger(), "Parameter can_interface_name could not be read."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (!this->get_parameter("master_config", dcf_txt_)) { RCLCPP_ERROR(this->get_logger(), "Parameter master_config could not be read."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (!this->get_parameter("master_bin", dcf_bin_)) { RCLCPP_ERROR(this->get_logger(), "Parameter master_bin could not be read."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (!this->get_parameter("bus_config", bus_config_)) { RCLCPP_ERROR(this->get_logger(), "Parameter bus_config could not be read."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (can_interface_name_.length() == 0) { RCLCPP_ERROR(this->get_logger(), "Parameter can_interface_name was not set."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (dcf_txt_.length() == 0) { RCLCPP_ERROR(this->get_logger(), "Parameter master_config was not set."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } if (bus_config_.length() == 0) { RCLCPP_ERROR(this->get_logger(), "Parameter bus_config was not set."); throw DeviceContainerException("Fatal: Getting Parameter failed."); } RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); RCLCPP_INFO(this->get_logger(), "\t can_interface_name %s", can_interface_name_.c_str()); try { this->config_ = std::make_unique(bus_config_); this->config_->init_config(); } catch (const std::exception & e) { RCLCPP_ERROR(this->get_logger(), "Loading bus configuration %s failed", bus_config_.c_str()); throw DeviceContainerException("Fatal: Loading bus configuration " + bus_config_ + " failed."); } } bool DeviceContainer::load_master() { RCLCPP_INFO(this->get_logger(), "Loading Master Configuration."); std::vector devices; uint32_t count = this->config_->get_all_devices(devices); if (count == 0) { return false; } bool master_found = false; // Find master in configuration for (auto it = devices.begin(); it != devices.end(); it++) { if (it->find("master") != std::string::npos && !master_found) { auto node_id = config_->get_entry(*it, "node_id"); auto driver_name = config_->get_entry(*it, "driver"); auto package_name = config_->get_entry(*it, "package"); auto node_namespace = config_->get_entry(*it, "namespace").value_or(""); if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) { RCLCPP_ERROR( this->get_logger(), "Error: Bus Configuration has incomplete configuration for master"); return false; } std::vector params; params.push_back(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); params.push_back(rclcpp::Parameter("master_dcf", this->dcf_txt_)); params.push_back(rclcpp::Parameter("master_bin", this->dcf_bin_)); params.push_back(rclcpp::Parameter("can_interface_name", this->can_interface_name_)); params.push_back(rclcpp::Parameter("node_id", (int)node_id.value())); params.push_back(rclcpp::Parameter("non_transmit_timeout", 100)); params.push_back(rclcpp::Parameter("config", config_->dump_device(*it))); if (!this->load_component( package_name.value(), driver_name.value(), node_id.value(), *it, params, node_namespace)) { RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed."); return false; } add_node_to_executor(can_master_->get_node_base_interface()); can_master_->init(); master_found = true; can_master_id_ = node_id.value(); this->lifecycle_operation_ = can_master_->is_lifecycle(); } } if (!master_found) { RCLCPP_ERROR(this->get_logger(), "Error: Master not in configuration"); return false; } return true; } bool DeviceContainer::load_drivers() { RCLCPP_INFO(this->get_logger(), "Loading Driver Configuration."); std::vector devices; uint32_t count = this->config_->get_all_devices(devices); if (count == 0) { return false; } for (auto it = devices.begin(); it != devices.end(); it++) { if (it->find("master") == std::string::npos && it->find("options") == std::string::npos) { auto node_id = config_->get_entry(*it, "node_id"); auto driver_name = config_->get_entry(*it, "driver"); auto package_name = config_->get_entry(*it, "package"); auto node_namespace = config_->get_entry(*it, "namespace").value_or(""); if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) { RCLCPP_ERROR( this->get_logger(), "Error: Bus Configuration has incomplete configuration for %s", it->c_str()); return false; } if (node_id.value() == can_master_id_) { RCLCPP_ERROR( this->get_logger(), "Error: Bus Configuration has duplicate entry for node id %i", node_id.value()); return false; } if (registered_drivers_.count(node_id.value()) != 0) { RCLCPP_ERROR( this->get_logger(), "Error: Bus Configuration has duplicate entry for node id %i", node_id.value()); return false; } RCLCPP_INFO( this->get_logger(), "Found device %s with driver %s", it->c_str(), driver_name.value().c_str()); std::vector params; params.push_back(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); params.push_back(rclcpp::Parameter("node_id", (int)node_id.value())); params.push_back(rclcpp::Parameter("config", config_->dump_device(*it))); params.push_back(rclcpp::Parameter("non_transmit_timeout", 100)); if (!this->load_component( package_name.value(), driver_name.value(), node_id.value(), *it, params, node_namespace)) { RCLCPP_ERROR( this->get_logger(), "Loading driver failed: node_id(%hu), node_name(%s), driver(%s), driver_package(%s)", node_id.value(), it->c_str(), driver_name.value().c_str(), package_name.value().c_str()); return false; } add_node_to_executor(registered_drivers_[node_id.value()]->get_node_base_interface()); registered_drivers_[node_id.value()]->init(); } } return true; } bool DeviceContainer::load_manager() { if (this->lifecycle_operation_) { RCLCPP_INFO(this->get_logger(), "Loading Manager Configuration."); auto node_options = rclcpp::NodeOptions(); node_options.use_global_arguments(false); // Set parameters rclcpp::Parameter container_name("container_name", this->get_fully_qualified_name()); std::vector params; params.push_back(container_name); node_options.parameter_overrides(params); // Instantiate Manager this->lifecycle_manager_ = std::make_unique(node_options); // Add to executor add_node_to_executor(this->lifecycle_manager_->get_node_base_interface()); // Initialise based on configuration file. this->lifecycle_manager_->init(this->config_); } return true; } void DeviceContainer::init() { configure(); if (!this->load_master()) { throw DeviceContainerException("Fatal: Loading Master Failed."); } if (!this->load_drivers()) { throw DeviceContainerException("Fatal: Loading Drivers Failed."); } if (!this->load_manager()) { throw DeviceContainerException("Fatal: Loading Manager Failed."); } } void DeviceContainer::init( const std::string & can_interface_name, const std::string & master_config, const std::string & bus_config, const std::string & master_bin) { can_interface_name_ = can_interface_name; dcf_txt_ = master_config; dcf_bin_ = master_bin; bus_config_ = bus_config; RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); RCLCPP_INFO(this->get_logger(), "\t can_interface_name %s", can_interface_name_.c_str()); RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); this->config_ = std::make_unique(bus_config_); this->config_->init_config(); if (!this->load_master()) { throw DeviceContainerException("Fatal: Loading Master Failed."); } if (!this->load_drivers()) { throw DeviceContainerException("Fatal: Loading Drivers Failed."); } if (!this->load_manager()) { throw DeviceContainerException("Fatal: Loading Manager Failed."); } } void DeviceContainer::on_list_nodes( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { auto components = list_components(); for (auto it = components.begin(); it != components.end(); ++it) { RCLCPP_INFO(this->get_logger(), "%hu - %s", it->first, it->second.c_str()); response->unique_ids.push_back((uint64_t)it->first); response->full_node_names.push_back(it->second); } } std::map DeviceContainer::list_components() { std::map components; components[can_master_id_] = can_master_->get_node_base_interface()->get_fully_qualified_name(); if (this->lifecycle_operation_) { components[256] = lifecycle_manager_->get_node_base_interface()->get_fully_qualified_name(); } for (auto & driver : registered_drivers_) { components[driver.first] = driver.second->get_node_base_interface()->get_fully_qualified_name(); } return components; } // int main(int argc, char const *argv[]) // { // rclcpp::init(argc, argv); // auto exec = std::make_shared(); // auto lifecycle_device_container_node = std::make_shared(exec); // exec->add_node(lifecycle_device_container_node); // std::thread spinThread([&exec]() // { exec->spin(); }); // std::this_thread::sleep_for(100ms); // if (lifecycle_device_container_node->init()) // { // RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation successful."); // } // else // { // RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation failed."); // } // spinThread.join(); // return 0; // } // bool DeviceContainer::set_remote_paramter(std::string node_name, rclcpp::Parameter ¶m) // { // std::string service_name = node_name + "/set_parameters"; // rclcpp::Client::SharedPtr set_parameter_client; // set_parameter_client = node_->create_client( // service_name); // while (!set_parameter_client->wait_for_service(non_transmit_timeout_)) // { // if (!rclcpp::ok()) // { // RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for init_driver service. // Exiting."); // } // RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); // } // auto request = std::make_shared(); // request->parameters.push_back(param.to_parameter_msg()); // auto future_result = demand_set_master_client->async_send_request(request); // auto future_status = future_result.wait_for(std::chrono::milliseconds(1000)); // RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); // if (future_status == std::future_status::ready) // { // future_result.get(); // } // } ================================================ FILE: canopen_core/src/device_container_error.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/device_container_error.hpp" #include namespace ros2_canopen { char * DeviceContainerException::what() { char * res = new char[1000]; strcpy(res, what_.c_str()); return res; } } // namespace ros2_canopen ================================================ FILE: canopen_core/src/device_container_node.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/device_container.hpp" using namespace ros2_canopen; int main(int argc, char const * argv[]) { rclcpp::init(argc, argv); auto exec = std::make_shared(); auto device_container = std::make_shared(exec); std::thread spinThread([&device_container]() { device_container->init(); }); exec->add_node(device_container); exec->spin(); spinThread.join(); device_container->shutdown(); rclcpp::shutdown(); return 0; } ================================================ FILE: canopen_core/src/driver_error.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/driver_error.hpp" #include namespace ros2_canopen { char * DriverException::what() { char * res = new char[1000]; strcpy(res, what_.c_str()); return res; } } // namespace ros2_canopen ================================================ FILE: canopen_core/src/driver_node.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/driver_node.hpp" using namespace ros2_canopen; void CanopenDriver::init() { node_canopen_driver_->init(); node_canopen_driver_->configure(); bool success = false; for (int i = 0; i < 5; i++) { try { node_canopen_driver_->demand_set_master(); success = true; break; } catch (std::exception & e) { RCLCPP_WARN( this->get_logger(), "Failed to get demand set master result becauss %s. Retrying.", e.what()); } } if (!success) { RCLCPP_WARN(this->get_logger(), "Failed to get demand set master result. Exiting..."); throw DriverException("Failed to get demand set master result. Exiting..."); } node_canopen_driver_->activate(); } void CanopenDriver::shutdown() { node_canopen_driver_->shutdown(); } void CanopenDriver::set_master( std::shared_ptr exec, std::shared_ptr master) { node_canopen_driver_->set_master(exec, master); } void LifecycleCanopenDriver::init() { node_canopen_driver_->init(); } void LifecycleCanopenDriver::shutdown() { node_canopen_driver_->shutdown(); } void LifecycleCanopenDriver::set_master( std::shared_ptr exec, std::shared_ptr master) { node_canopen_driver_->set_master(exec, master); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenDriver::on_configure(const rclcpp_lifecycle::State & state) { node_canopen_driver_->configure(); bool success = false; for (int i = 0; i < 5; i++) { try { node_canopen_driver_->demand_set_master(); success = true; break; } catch (std::exception & e) { RCLCPP_WARN( this->get_logger(), "Failed to get demand set master result becauss %s. Retrying.", e.what()); } } if (!success) { RCLCPP_WARN(this->get_logger(), "Failed to get demand set master result. Exiting..."); throw DriverException("Failed to get demand set master result. Exiting..."); } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenDriver::on_activate(const rclcpp_lifecycle::State & state) { node_canopen_driver_->activate(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenDriver::on_deactivate(const rclcpp_lifecycle::State & state) { node_canopen_driver_->deactivate(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenDriver::on_cleanup(const rclcpp_lifecycle::State & state) { node_canopen_driver_->cleanup(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenDriver::on_shutdown(const rclcpp_lifecycle::State & state) { node_canopen_driver_->shutdown(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } ================================================ FILE: canopen_core/src/lifecycle_manager.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/lifecycle_manager.hpp" namespace ros2_canopen { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State & state) { this->get_parameter("container_name", this->container_name_); bool res = this->load_from_config(); if (!res) { RCLCPP_ERROR(this->get_logger(), "Failed to load from config"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State & state) { if (!this->bring_up_all()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_deactivate(const rclcpp_lifecycle::State & state) { if (!this->bring_down_all()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_cleanup(const rclcpp_lifecycle::State & state) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_shutdown(const rclcpp_lifecycle::State & state) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } void LifecycleManager::init(std::shared_ptr config) { this->config_ = config; } bool LifecycleManager::load_from_config() { std::vector devices; uint32_t count = this->config_->get_all_devices(devices); RCLCPP_INFO(this->get_logger(), "Configuring for %u devices.", count); // Find master in configuration for (auto it = devices.begin(); it != devices.end(); it++) { uint8_t node_id = config_->get_entry(*it, "node_id").value(); std::string change_state_client_name = *it; std::string get_state_client_name = *it; get_state_client_name += "/get_state"; change_state_client_name += "/change_state"; RCLCPP_INFO(this->get_logger(), "Found %s (node_id=%hu)", it->c_str(), node_id); device_names_to_ids.emplace(*it, node_id); rclcpp::Client::SharedPtr get_state_client = this->create_client( get_state_client_name, rclcpp::QoS(10), cbg_clients); rclcpp::Client::SharedPtr change_state_client = this->create_client( change_state_client_name, rclcpp::QoS(10), cbg_clients); this->drivers_get_state_clients.emplace(node_id, get_state_client); this->drivers_change_state_clients.emplace(node_id, change_state_client); if (it->find("master") != std::string::npos) { this->master_id_ = node_id; } } return true; } unsigned int LifecycleManager::get_state(uint8_t node_id, std::chrono::seconds time_out) { auto request = std::make_shared(); auto client = this->drivers_get_state_clients[node_id]; if (!client->wait_for_service(time_out)) { RCLCPP_ERROR(get_logger(), "Service %s is not available.", client->get_service_name()); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } // We send the service request for asking the current // state of the lc_talker node. auto future_result = client->async_send_request(request); // Let's wait until we have the answer from the node. // If the request times out, we return an unknown state. auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { RCLCPP_ERROR( get_logger(), "Server time out while getting current state for node 0x%X", node_id); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } auto result = future_result.get()->current_state; return result.id; } bool LifecycleManager::change_state( uint8_t node_id, uint8_t transition, std::chrono::seconds time_out) { auto client = this->drivers_change_state_clients[node_id]; auto request = std::make_shared(); request->transition.id = transition; if (!client->wait_for_service(time_out)) { RCLCPP_ERROR(get_logger(), "Service %s is not available.", client->get_service_name()); return false; } // We send the request with the transition we want to invoke. auto future_result = client->async_send_request(request); // Let's wait until we have the answer from the node. // If the request times out, we return an unknown state. auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { RCLCPP_ERROR( get_logger(), "Server time out while getting current state for node 0x%X", node_id); return false; } if (future_result.get()->success) { return true; } else { RCLCPP_WARN( get_logger(), "Failed to trigger transition %u", static_cast(transition)); return false; } return false; } bool LifecycleManager::bring_up_master() { auto state = this->get_state(master_id_, 3s); if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( this->get_logger(), "Failed to bring up master. Master not in unconfigured state."); return false; } RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state unconfigured.", master_id_); if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, 3s)) { RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Configure Transition failed."); return false; } RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state inactive.", master_id_); if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, 3s)) { RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Activate Transition failed."); return false; } RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state active.", master_id_); return true; } bool LifecycleManager::bring_down_master() { this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, 3s); this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, 3s); auto state = this->get_state(master_id_, 3s); if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { return false; } return true; } bool LifecycleManager::bring_up_driver(std::string device_name) { auto node_id = this->device_names_to_ids[device_name]; RCLCPP_DEBUG(this->get_logger(), "Bringing up %s with id %u", device_name.c_str(), node_id); auto master_state = this->get_state(master_id_, 3s); if (master_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( this->get_logger(), "Failed to bring up %s. Master not in active state.", device_name.c_str()); return false; } auto state = this->get_state(node_id, 3s); if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_ERROR( this->get_logger(), "Failed to bring up %s. Not in unconfigured state.", device_name.c_str()); return false; } RCLCPP_DEBUG( this->get_logger(), "%s (node_id=%hu) has state unconfigured. Attempting to configure.", device_name.c_str(), node_id); if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, 3s)) { RCLCPP_ERROR( this->get_logger(), "Failed to bring up %s. Configure Transition failed.", device_name.c_str()); return false; } RCLCPP_DEBUG( this->get_logger(), "%s (node_id=%hu) has state inactive. Attempting to activate.", device_name.c_str(), node_id); if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, 3s)) { RCLCPP_ERROR( this->get_logger(), "Failed to bring up %s. Activate Transition failed.", device_name.c_str()); return false; } RCLCPP_DEBUG( this->get_logger(), "%s (node_id=%hu) has state active.", device_name.c_str(), node_id); return true; } bool LifecycleManager::bring_down_driver(std::string device_name) { auto node_id = this->device_names_to_ids[device_name]; this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, 3s); this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, 3s); auto state = this->get_state(node_id, 3s); if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { return false; } return true; } bool LifecycleManager::bring_up_all() { if (!this->bring_up_master()) { return false; } for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) { if (it->first.find("master") == std::string::npos) { if (!this->bring_up_driver(it->first)) { return false; } } else { RCLCPP_DEBUG(this->get_logger(), "Skipped master."); } } return true; } bool LifecycleManager::bring_down_all() { RCLCPP_INFO(this->get_logger(), "Bring Down all"); for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) { if (it->first.compare("master") != 0) { if (!this->bring_down_driver(it->first)) { return false; } } } if (!this->bring_down_master()) { return false; } return true; } } // namespace ros2_canopen #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleManager) ================================================ FILE: canopen_core/src/master_error.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/master_error.hpp" #include namespace ros2_canopen { char * MasterException::what() { char * res = new char[1000]; strcpy(res, what_.c_str()); return res; } } // namespace ros2_canopen ================================================ FILE: canopen_core/src/master_node.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/master_node.hpp" using namespace ros2_canopen; void CanopenMaster::init() { node_canopen_master_->init(); node_canopen_master_->configure(); node_canopen_master_->activate(); } void CanopenMaster::shutdown() { node_canopen_master_->shutdown(); } std::shared_ptr CanopenMaster::get_master() { return node_canopen_master_->get_master(); } std::shared_ptr CanopenMaster::get_executor() { return node_canopen_master_->get_executor(); } void LifecycleCanopenMaster::init() { node_canopen_master_->init(); } void LifecycleCanopenMaster::shutdown() { node_canopen_master_->shutdown(); } std::shared_ptr LifecycleCanopenMaster::get_master() { return node_canopen_master_->get_master(); } std::shared_ptr LifecycleCanopenMaster::get_executor() { return node_canopen_master_->get_executor(); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenMaster::on_configure(const rclcpp_lifecycle::State & state) { node_canopen_master_->configure(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenMaster::on_activate(const rclcpp_lifecycle::State & state) { node_canopen_master_->activate(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenMaster::on_deactivate(const rclcpp_lifecycle::State & state) { node_canopen_master_->deactivate(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenMaster::on_cleanup(const rclcpp_lifecycle::State & state) { node_canopen_master_->cleanup(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleCanopenMaster::on_shutdown(const rclcpp_lifecycle::State & state) { node_canopen_master_->shutdown(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } ================================================ FILE: canopen_core/src/node_interfaces/node_canopen_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/node_interfaces/node_canopen_driver.hpp" using namespace std::chrono_literals; template <> void ros2_canopen::node_interfaces::NodeCanopenDriver::demand_set_master() { RCLCPP_DEBUG(node_->get_logger(), "demand_set_master_start"); if (!configured_.load()) { throw ros2_canopen::DriverException("Set Master: driver is not configured"); } std::string init_service_name = container_name_ + "/init_driver"; RCLCPP_DEBUG(node_->get_logger(), "Service: %s", init_service_name.c_str()); rclcpp::Client::SharedPtr demand_set_master_client; // demand_set_master_client; demand_set_master_client = node_->create_client( init_service_name, rclcpp::QoS(10), client_cbg_); while (!demand_set_master_client->wait_for_service(non_transmit_timeout_)) { if (!rclcpp::ok()) { RCLCPP_ERROR( node_->get_logger(), "Interrupted while waiting for init_driver service. Exiting."); } RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); } auto request = std::make_shared(); request->nodeid = node_id_; auto future_result = demand_set_master_client->async_send_request(request); auto future_status = future_result.wait_for(non_transmit_timeout_); RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); if (future_status == std::future_status::ready) { future_result.get(); } else { RCLCPP_ERROR(node_->get_logger(), "Could not get result."); throw DriverException("Could not get result."); } } template <> void ros2_canopen::node_interfaces::NodeCanopenDriver< rclcpp_lifecycle::LifecycleNode>::demand_set_master() { RCLCPP_DEBUG(node_->get_logger(), "demand_set_master_start"); if (!configured_.load()) { throw ros2_canopen::DriverException("Set Master: driver is not configured"); } std::string init_service_name = container_name_ + "/init_driver"; rclcpp::Client::SharedPtr demand_set_master_client; // demand_set_master_client; demand_set_master_client = node_->create_client( init_service_name, rclcpp::QoS(10), client_cbg_); while (!demand_set_master_client->wait_for_service(non_transmit_timeout_)) { if (!rclcpp::ok()) { RCLCPP_ERROR( node_->get_logger(), "Interrupted while waiting for init_driver service. Exiting."); } RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); } auto request = std::make_shared(); request->nodeid = node_id_; auto future_result = demand_set_master_client->async_send_request(request); auto future_status = future_result.wait_for(non_transmit_timeout_); RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); if (future_status == std::future_status::ready) { future_result.get(); } else { RCLCPP_ERROR(node_->get_logger(), "Could not get result."); throw DriverException("Could not get result."); } } template class ros2_canopen::node_interfaces::NodeCanopenDriver; template class ros2_canopen::node_interfaces::NodeCanopenDriver; ================================================ FILE: canopen_core/src/node_interfaces/node_canopen_master.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/node_interfaces/node_canopen_master.hpp" template class ros2_canopen::node_interfaces::NodeCanopenMaster; template class ros2_canopen::node_interfaces::NodeCanopenMaster; ================================================ FILE: canopen_core/test/CMakeLists.txt ================================================ ament_add_gmock(test_node_canopen_driver test_node_canopen_driver.cpp ) target_include_directories(test_node_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_canopen_driver ${canopen_interfaces_TARGETS} node_canopen_driver rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) ament_add_gmock(test_node_canopen_master test_node_canopen_master.cpp ) target_include_directories(test_node_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_canopen_master ${canopen_interfaces_TARGETS} node_canopen_master rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) # Temporary fixing ci build stability if(DEVICE_CONTAINER_TESTING) ament_add_gmock(test_device_container test_device_container.cpp ) target_include_directories(test_device_container PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_device_container ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} device_container node_canopen_driver node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) endif() file(COPY bus_configs DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) ament_add_gmock(test_canopen_driver test_canopen_driver.cpp ) target_include_directories(test_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_canopen_driver ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} node_canopen_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) ament_add_gmock(test_lifecycle_canopen_driver test_lifecycle_canopen_driver.cpp ) target_include_directories(test_lifecycle_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_lifecycle_canopen_driver ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} node_canopen_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) ament_add_gmock(test_canopen_master test_canopen_master.cpp ) target_include_directories(test_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_canopen_master ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) ament_add_gmock(test_lifecycle_canopen_master test_lifecycle_canopen_master.cpp ) target_include_directories(test_lifecycle_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_lifecycle_canopen_master ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle yaml-cpp ) ament_add_gmock(test_lifecycle_manager test_lifecycle_manager.cpp ) target_include_directories(test_lifecycle_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_lifecycle_manager ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} device_container rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gmock(test_errors test_errors.cpp ) target_include_directories(test_errors PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_errors ${canopen_interfaces_TARGETS} ${lifecycle_msgs_TARGETS} device_container node_canopen_driver node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ================================================ FILE: canopen_core/test/bus_configs/bad_driver_duplicate.yml ================================================ --- proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/bad_driver_no_driver.yml ================================================ --- proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/bad_driver_no_id.yml ================================================ --- proxy_device_1: dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/bad_driver_no_package.yml ================================================ --- proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" ================================================ FILE: canopen_core/test/bus_configs/bad_master_no_driver.yml ================================================ --- master: node_id: 1 package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/bad_master_no_id.yml ================================================ --- master: driver: "ros2_canopen::CanopenMaster" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/bad_master_no_package.yml ================================================ --- master: node_id: 1 driver: "ros2_canopen::CanopenMaster" ================================================ FILE: canopen_core/test/bus_configs/bad_no_master.yml ================================================ --- proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" proxy_device_2: node_id: 3 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/good_driver.yml ================================================ --- proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/good_master.yml ================================================ --- master: node_id: 1 driver: "ros2_canopen::CanopenMaster" package: "canopen_core" ================================================ FILE: canopen_core/test/bus_configs/good_master_and_two_driver.yml ================================================ --- master: node_id: 1 driver: "ros2_canopen::CanopenMaster" package: "canopen_core" proxy_device_1: node_id: 2 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" proxy_device_2: node_id: 3 dcf: "simple.eds" dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" driver: "ros2_canopen::CanopenDriver" package: "canopen_core" ================================================ FILE: canopen_core/test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=2 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=32 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1A00 20=0x1A01 21=0x1F25 22=0x1F55 23=0x1F80 24=0x1F81 25=0x1F82 26=0x1F84 27=0x1F85 28=0x1F86 29=0x1F87 30=0x1F88 31=0x1F89 32=0x1F8A [ManufacturerObjects] SupportedObjects=12 1=0x2000 2=0x2001 3=0x2200 4=0x2201 5=0x5800 6=0x5801 7=0x5A00 8=0x5A01 9=0x5C00 10=0x5C01 11=0x5E00 12=0x5E01 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=2 2=0x00000082 3=0x00000083 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=2 2=0x00000005 3=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=2 2=0x00000360 3=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=proxy_device_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=proxy_device_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=proxy_device_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=proxy_device_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_core/test/simple.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=11 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1A00 [ManufacturerObjects] SupportedObjects=2 1=0x4000 2=0x4001 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 ================================================ FILE: canopen_core/test/test_canopen_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/driver_node.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenDriverInterface { public: MOCK_METHOD( void, set_master, (std::shared_ptr exec, std::shared_ptr master), (override)); MOCK_METHOD(void, demand_set_master, (), (override)); MOCK_METHOD(void, init, (), (override)); MOCK_METHOD(void, configure, (), (override)); MOCK_METHOD(void, activate, (), (override)); MOCK_METHOD(void, deactivate, (), (override)); MOCK_METHOD(void, cleanup, (), (override)); MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(void, add_to_master, (), (override)); MOCK_METHOD(void, remove_from_master, (), (override)); }; class MockCanopenMaster : public CanopenDriver { friend class CanopenDriverTest; FRIEND_TEST(CanopenDriverTest, test_init); }; class CanopenDriverTest : public testing::Test { public: std::shared_ptr canopen_driver; std::shared_ptr node_canopen_driver; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "SetUp"); rclcpp::init(0, nullptr); canopen_driver = std::make_shared(); node_canopen_driver = std::make_shared(); canopen_driver->node_canopen_driver_ = std::static_pointer_cast( node_canopen_driver); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(CanopenDriverTest, test_init) { EXPECT_CALL(*node_canopen_driver, init()).Times(1); EXPECT_CALL(*node_canopen_driver, configure()).Times(1); EXPECT_CALL(*node_canopen_driver, demand_set_master()).Times(1); EXPECT_CALL(*node_canopen_driver, activate()).Times(1); canopen_driver->init(); } TEST_F(CanopenDriverTest, test_set_master) { std::shared_ptr exec; std::shared_ptr master; EXPECT_CALL(*node_canopen_driver, set_master(_, _)).Times(1); EXPECT_NO_THROW(canopen_driver->set_master(exec, master)); } TEST_F(CanopenDriverTest, test_get_node_base_interface) { auto base_iface = canopen_driver->get_node_base_interface(); EXPECT_TRUE(base_iface); } TEST_F(CanopenDriverTest, test_shutdown) { EXPECT_CALL(*node_canopen_driver, shutdown()); EXPECT_NO_THROW(canopen_driver->shutdown()); } TEST_F(CanopenDriverTest, test_is_lifecycle) { EXPECT_FALSE(canopen_driver->is_lifecycle()); } TEST_F(CanopenDriverTest, test_get_node_canopen_driver_interface) { EXPECT_TRUE(canopen_driver->get_node_canopen_driver_interface() == node_canopen_driver); } ================================================ FILE: canopen_core/test/test_canopen_master.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/master_node.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenMasterInterface { public: MOCK_METHOD(void, init, (), (override)); MOCK_METHOD(void, configure, (), (override)); MOCK_METHOD(void, activate, (), (override)); MOCK_METHOD(void, deactivate, (), (override)); MOCK_METHOD(void, cleanup, (), (override)); MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(std::shared_ptr, get_master, (), (override)); MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); }; class MockCanopenMaster : public CanopenMaster { friend class CanopenMasterTest; FRIEND_TEST(CanopenMasterTest, test_init); }; class CanopenMasterTest : public testing::Test { public: std::shared_ptr canopen_master; std::shared_ptr node_canopen_master; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); canopen_master = std::make_shared(); node_canopen_master = std::make_shared(); canopen_master->node_canopen_master_ = std::static_pointer_cast( node_canopen_master); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(CanopenMasterTest, test_init) { EXPECT_CALL(*node_canopen_master, init()).Times(1); EXPECT_CALL(*node_canopen_master, configure()).Times(1); EXPECT_CALL(*node_canopen_master, activate()).Times(1); canopen_master->init(); } TEST_F(CanopenMasterTest, test_get_node_base_interface) { auto base_iface = canopen_master->get_node_base_interface(); EXPECT_TRUE(base_iface); } TEST_F(CanopenMasterTest, test_shutdown) { EXPECT_CALL(*node_canopen_master, shutdown()); EXPECT_NO_THROW(canopen_master->shutdown()); } TEST_F(CanopenMasterTest, test_is_lifecycle) { EXPECT_FALSE(canopen_master->is_lifecycle()); } TEST_F(CanopenMasterTest, test_get_master) { EXPECT_CALL(*node_canopen_master, get_master()); EXPECT_NO_THROW(canopen_master->get_master()); } TEST_F(CanopenMasterTest, test_get_executor) { EXPECT_CALL(*node_canopen_master, get_executor()); EXPECT_NO_THROW(canopen_master->get_executor()); } ================================================ FILE: canopen_core/test/test_device_container.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/device_container.hpp" #include "canopen_core/device_container_error.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockNodeFactory : public rclcpp_components::NodeFactory { public: MOCK_METHOD( rclcpp_components::NodeInstanceWrapper, create_node_instance, (const rclcpp::NodeOptions & options), (override)); }; class FakeDeviceContainer { public: virtual std::vector get_component_resources( const std::string & package_name, const std::string & resource_index = "rclcpp_components") const { // RCLCPP_INFO(this->get_logger(), "get_component_resources"); std::vector components; components.push_back(rclcpp_components::ComponentManager::ComponentResource( "ros2_canopen::CanopenMaster", "canopen_core")); components.push_back(rclcpp_components::ComponentManager::ComponentResource( "ros2_canopen::LifecycleCanopenMaster", "canopen_core")); components.push_back(rclcpp_components::ComponentManager::ComponentResource( "ros2_canopen::CanopenDriver", "canopen_core")); components.push_back(rclcpp_components::ComponentManager::ComponentResource( "ros2_canopen::LifecycleCanopenDriver", "canopen_core")); // get_component_resources_mock(true); return components; } }; using namespace std::placeholders; class MockDeviceContainer : public DeviceContainer { FakeDeviceContainer fake_device_container; public: FRIEND_TEST(DeviceContainerTest, test_load_component_master); FRIEND_TEST(DeviceContainerTest, test_load_component_driver_non_lifecycle); FRIEND_TEST(DeviceContainerTest, test_load_component_driver_lifecycle); FRIEND_TEST(DeviceContainerTest, test_get_registered_drivers); FRIEND_TEST(DeviceContainerTest, test_shutdown); FRIEND_TEST(DeviceContainerTest, test_get_ids_of_drivers_with_type); FRIEND_TEST(DeviceContainerTest, test_get_driver_type); FRIEND_TEST(DeviceContainerTest, test_on_list_nodes); FRIEND_TEST(DeviceContainerTest, test_load_master_good); FRIEND_TEST(DeviceContainerTest, test_load_master_load_component_fail); FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_id); FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_driver); FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_package); FRIEND_TEST(DeviceContainerTest, test_load_driver_good); FRIEND_TEST(DeviceContainerTest, test_load_driver_load_component_fail); FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_id); FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_package); FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_driver); FRIEND_TEST(DeviceContainerTest, test_load_manager_no_lifecycle); FRIEND_TEST(DeviceContainerTest, test_load_manager_lifecycle); FRIEND_TEST(DeviceContainerTest, test_configure_good); MockDeviceContainer( std::weak_ptr executor = std::weak_ptr()) : DeviceContainer(executor), fake_device_container() { } MOCK_METHOD( bool, load_component, (std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name, std::vector & params), (override)); MOCK_METHOD(void, configure, (), (override)); MOCK_METHOD(bool, load_master, (), (override)); MOCK_METHOD(bool, load_drivers, (), (override)); MOCK_METHOD(bool, load_manager, (), (override)); MOCK_METHOD( std::shared_ptr, create_component_factory, (const ComponentResource & resource), (override)); MOCK_METHOD( std::vector, get_component_resources, (const std::string & package_name, const std::string & resource_index), (const, override)); MOCK_METHOD( void, add_node_to_executor, (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface), (override)); void DelegateToFake() { ON_CALL(*this, load_component) .WillByDefault( [this]( std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name, std::vector & params) -> bool { return DeviceContainer::load_component( package_name, driver_name, node_id, node_name, params); }); ON_CALL(*this, load_master) .WillByDefault([this]() -> bool { return DeviceContainer::load_master(); }); ON_CALL(*this, load_drivers) .WillByDefault([this]() -> bool { return DeviceContainer::load_drivers(); }); ON_CALL(*this, load_manager) .WillByDefault([this]() -> bool { return DeviceContainer::load_manager(); }); ON_CALL(*this, configure).WillByDefault([this]() { return DeviceContainer::configure(); }); ON_CALL(*this, get_component_resources) .WillByDefault( [this](const std::string & package_name, const std::string & resource_index) -> std::vector { return fake_device_container.get_component_resources(package_name, resource_index); }); ON_CALL(*this, add_node_to_executor) .WillByDefault([this](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface) { DeviceContainer::add_node_to_executor(node_interface); }); } }; class MockCanopenDriver : public CanopenDriverInterface, public rclcpp::Node { public: explicit MockCanopenDriver( std::string name = "mock_canopen_driver", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp::Node(name, node_options) { } MOCK_METHOD(void, init, (), (override)); MOCK_METHOD( void, set_master, (std::shared_ptr exec, std::shared_ptr master), (override)); // MOCK_METHOD(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr, get_node_base_interface, (), // (override)); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() { return rclcpp::Node::get_node_base_interface(); } MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(bool, is_lifecycle, (), (override)); MOCK_METHOD( std::shared_ptr, get_node_canopen_driver_interface, (), (override)); }; class MockCanopenMaster : public CanopenMasterInterface, public rclcpp::Node { public: explicit MockCanopenMaster( std::string name = "mock_canopen_master", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) : rclcpp::Node(name, node_options) { } MOCK_METHOD(void, init, (), (override)); MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(std::shared_ptr, get_master, (), (override)); MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() { return rclcpp::Node::get_node_base_interface(); } MOCK_METHOD(bool, is_lifecycle, (), (override)); }; class DeviceContainerTest : public testing::Test { public: std::shared_ptr device_container; std::shared_ptr exec; std::thread spinThread; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "SetUp"); rclcpp::init(0, nullptr); exec = std::make_shared(); device_container = std::make_shared(exec); device_container->DelegateToFake(); exec->add_node(device_container); spinThread = std::thread([this]() { exec->spin(); }); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Teardown"); exec->cancel(); spinThread.join(); RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Executor joined."); exec->remove_node(device_container); RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Node removed."); rclcpp::shutdown(); } }; TEST_F(DeviceContainerTest, test_name_and_parameters_declared) { EXPECT_TRUE(std::string("device_container").compare(device_container->get_name()) == 0); EXPECT_TRUE(device_container->has_parameter("can_interface_name")); EXPECT_TRUE(device_container->has_parameter("master_config")); EXPECT_TRUE(device_container->has_parameter("bus_config")); EXPECT_TRUE(device_container->has_parameter("master_bin")); } TEST_F(DeviceContainerTest, test_services_declared) { auto services = device_container->get_service_names_and_types_by_node( device_container->get_name(), device_container->get_namespace()); bool found_init_driver = false; bool found_unload_node = false; bool found_load_node = false; for (auto it = services.begin(); it != services.end(); ++it) { if (it->first.compare("/device_container/init_driver") == 0) { found_init_driver = true; } if (it->first.compare("/device_container/_container/unload_node") == 0) { found_unload_node = true; } if (it->first.compare("/device_container/_container/load_node") == 0) { found_load_node = true; } } EXPECT_TRUE(found_init_driver); EXPECT_FALSE(found_unload_node); EXPECT_FALSE(found_load_node); } TEST_F(DeviceContainerTest, test_load_component_master) { std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenMaster"); std::string node_name("master"); uint16_t node_id = 1; auto node = std::make_shared(); auto node_factory = std::make_shared(); auto node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(node), std::bind(&MockCanopenMaster::get_node_base_interface, node)); EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); device_container->load_component(package, driver_name, node_id, node_name, params); // Check that can_master_ is assigned. EXPECT_TRUE(((long)device_container->can_master_.get() == (long)node.get())); // Check that master is not stored in driver map. EXPECT_FALSE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); } TEST_F(DeviceContainerTest, test_load_component_driver_non_lifecycle) { // Lifecycle operation false device_container->lifecycle_operation_ = false; std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenDriver"); std::string node_name("driver"); uint16_t node_id = 1; auto node = std::make_shared(); auto node_factory = std::make_shared(); auto node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(node), std::bind(&MockCanopenDriver::get_node_base_interface, node)); // Lifecycle component leads to throw EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(true)); EXPECT_THROW( device_container->load_component(package, driver_name, node_id, node_name, params), DeviceContainerException); // Non lifecycle component leads to normal behaviour EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); // Check that can_master_ is not assigned. EXPECT_FALSE(((long)device_container->can_master_.get() == (long)node.get())); // Check that driver is stored in driver map. EXPECT_TRUE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); } TEST_F(DeviceContainerTest, test_load_component_driver_lifecycle) { // Lifecycle operation true device_container->lifecycle_operation_ = true; std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenDriver"); std::string node_name("driver"); uint16_t node_id = 1; auto node = std::make_shared(); auto node_factory = std::make_shared(); auto node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(node), std::bind(&MockCanopenDriver::get_node_base_interface, node)); // Non lifecycle component leads to throw EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); EXPECT_THROW( device_container->load_component(package, driver_name, node_id, node_name, params), DeviceContainerException); // Lifecycle component leads to normal behaviour node_id = 2; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(true)); device_container->load_component(package, driver_name, node_id, node_name, params); // Check that can_master_ is not assigned. EXPECT_FALSE(((long)device_container->can_master_.get() == (long)node.get())); // Check that driver is stored in driver map. EXPECT_TRUE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); } TEST_F(DeviceContainerTest, test_get_registered_drivers) { // Lifecycle operation true device_container->lifecycle_operation_ = false; std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenDriver"); std::string node_name("driver"); uint16_t node_id = 1; auto node = std::make_shared(); auto node_factory = std::make_shared(); auto node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(node), std::bind(&MockCanopenDriver::get_node_base_interface, node)); // Should be zero at start EXPECT_EQ(device_container->get_registered_drivers().size(), 0U); // Load component and expect that size of registered drivers changes to 1 EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); EXPECT_EQ(device_container->get_registered_drivers().size(), 1U); // Load component and expect that size of registered drivers changes to 2 node_id = 2; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); EXPECT_EQ(device_container->get_registered_drivers().size(), 2U); } TEST_F(DeviceContainerTest, test_count_drivers) { std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenDriver"); std::string node_name("driver1"); uint16_t node_id = 1; auto node = std::make_shared(); auto node_factory = std::make_shared(); auto node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(node), std::bind(&MockCanopenDriver::get_node_base_interface, node)); // Should be zero at start EXPECT_EQ(device_container->count_drivers(), 0U); // Load component and expect that size of registered drivers changes to 1 EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); EXPECT_EQ(device_container->count_drivers(), 1U); // Load component and expect that size of registered drivers changes to 2 node_id = 2; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); EXPECT_EQ(device_container->count_drivers(), 2U); } TEST_F(DeviceContainerTest, test_shutdown) { // Lifecycle operation true std::vector params; std::string package("canopen_core"); std::string driver_name("ros2_canopen::CanopenMaster"); std::string node_name("master"); uint16_t node_id = 1; auto master_node = std::make_shared(); auto driver_node = std::make_shared(); auto node_factory = std::make_shared(); auto master_node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(master_node), std::bind(&MockCanopenMaster::get_node_base_interface, master_node)); auto driver_node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(driver_node), std::bind(&MockCanopenDriver::get_node_base_interface, driver_node)); // Load component and expect that size of registered drivers changes to 1 EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)) .Times(1) .WillOnce(Return(master_node_instance)); device_container->load_component(package, driver_name, node_id, node_name, params); driver_name = "ros2_canopen::CanopenDriver"; node_name = "driver2"; node_id = 2; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)) .Times(1) .WillOnce(Return(driver_node_instance)); EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); node_id = 3; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)) .Times(1) .WillOnce(Return(driver_node_instance)); EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, node_name, params); EXPECT_EQ(device_container->get_registered_drivers().size(), 2U); EXPECT_CALL(*driver_node, shutdown()).Times(2); EXPECT_CALL(*master_node, shutdown()).Times(1); device_container->shutdown(); } TEST_F(DeviceContainerTest, test_get_ids_of_drivers_with_type) { std::string file_name("bus_configs/good_master_and_two_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); std::string driver_name("ros2_canopen::CanopenDriver"); auto ids = device_container->get_ids_of_drivers_with_type(driver_name); EXPECT_EQ(ids.size(), 2U); } TEST_F(DeviceContainerTest, test_get_driver_type) { std::string file_name("bus_configs/good_master_and_two_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); uint16_t node_id = 2; auto driver_type = device_container->get_driver_type(node_id); EXPECT_EQ(driver_type.compare("ros2_canopen::CanopenDriver"), 0); } TEST_F(DeviceContainerTest, test_on_list_nodes) { // Lifecycle operation true std::vector params; std::string package("canopen_core"); std::string master_name("ros2_canopen::CanopenMaster"); std::string driver_name("ros2_canopen::CanopenDriver"); std::string master_node_name("master"); std::string driver_node_name("driver"); uint16_t node_id; auto master_node = std::make_shared(master_node_name); auto driver_node = std::make_shared(driver_node_name); device_container->lifecycle_manager_ = std::make_unique(rclcpp::NodeOptions()); auto node_factory = std::make_shared(); auto master_node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(master_node), std::bind(&MockCanopenMaster::get_node_base_interface, master_node)); auto driver_node_instance = rclcpp_components::NodeInstanceWrapper( std::static_pointer_cast(driver_node), std::bind(&MockCanopenDriver::get_node_base_interface, driver_node)); node_id = 1; device_container->can_master_id_ = node_id; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)) .Times(1) .WillOnce(Return(master_node_instance)); device_container->load_component(package, master_name, node_id, master_node_name, params); node_id = 2; EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); EXPECT_CALL(*device_container, get_component_resources(_, _)); EXPECT_CALL(*device_container, create_component_factory(_)) .Times(1) .WillOnce(Return(node_factory)); EXPECT_CALL(*node_factory, create_node_instance(_)) .Times(1) .WillOnce(Return(driver_node_instance)); EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); device_container->load_component(package, driver_name, node_id, driver_node_name, params); const std::shared_ptr request_header(nullptr); auto request = std::make_shared(); auto response = std::make_shared(); device_container->on_list_nodes(request_header, request, response); for (size_t i = 0; i < response->full_node_names.size(); i++) { if (response->full_node_names[i].compare(std::string("/").append(master_node_name)) == 0) { EXPECT_EQ(response->unique_ids[i], 1U); } else if (response->full_node_names[i].compare(std::string("/").append(driver_node_name)) == 0) { EXPECT_EQ(response->unique_ids[i], 2U); } else if (response->full_node_names[i].compare(std::string("/").append(driver_node_name)) == 0) { EXPECT_EQ(response->unique_ids[i], 256U); } else { RCLCPP_INFO(rclcpp::get_logger("TEST"), response->full_node_names[i].c_str()); FAIL(); } } } TEST_F(DeviceContainerTest, test_load_master_good) { std::string file_name("bus_configs/good_master.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto can_master = std::make_shared("master"); EXPECT_CALL(*device_container, load_master()); EXPECT_CALL(*device_container, load_component(_, _, _, _, _)) .WillOnce( [this, can_master]( std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name, std::vector & params) -> bool { device_container->can_master_ = std::static_pointer_cast(can_master); return true; }); EXPECT_CALL(*device_container, add_node_to_executor(_)); EXPECT_CALL(*can_master, init()); EXPECT_CALL(*can_master, is_lifecycle()); EXPECT_TRUE(device_container->load_master()); } TEST_F(DeviceContainerTest, test_load_master_load_component_fail) { std::string file_name("bus_configs/good_master.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto can_master = std::make_shared("master"); EXPECT_CALL(*device_container, load_master()); EXPECT_CALL(*device_container, load_component(_, _, _, _, _)).WillOnce(Return(false)); EXPECT_FALSE(device_container->load_master()); } TEST_F(DeviceContainerTest, test_load_master_bad_no_driver) { std::string file_name("bus_configs/bad_master_no_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto can_master = std::make_shared("master"); EXPECT_CALL(*device_container, load_master()); EXPECT_FALSE(device_container->load_master()); } TEST_F(DeviceContainerTest, test_load_master_bad_no_id) { std::string file_name("bus_configs/bad_master_no_id.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto can_master = std::make_shared("master"); EXPECT_CALL(*device_container, load_master()); EXPECT_FALSE(device_container->load_master()); } TEST_F(DeviceContainerTest, test_load_master_bad_no_package) { std::string file_name("bus_configs/bad_master_no_package.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto can_master = std::make_shared("master"); EXPECT_CALL(*device_container, load_master()); EXPECT_FALSE(device_container->load_master()); } TEST_F(DeviceContainerTest, test_load_driver_good) { std::string file_name("bus_configs/good_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto driver = std::make_shared("driver"); EXPECT_CALL(*device_container, load_drivers()); EXPECT_CALL(*device_container, load_component(_, _, _, _, _)) .WillOnce( [this, driver]( std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name, std::vector & params) -> bool { device_container->registered_drivers_[2] = std::static_pointer_cast(driver); return true; }); EXPECT_CALL(*device_container, add_node_to_executor(_)); EXPECT_CALL(*driver, init()); EXPECT_TRUE(device_container->load_drivers()); } TEST_F(DeviceContainerTest, test_load_driver_load_component_fail) { std::string file_name("bus_configs/good_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); auto driver = std::make_shared("driver"); EXPECT_CALL(*device_container, load_drivers()); EXPECT_CALL(*device_container, load_component(_, _, _, _, _)).WillOnce(Return(false)); EXPECT_FALSE(device_container->load_drivers()); } TEST_F(DeviceContainerTest, test_load_driver_bad_no_id) { std::string file_name("bus_configs/bad_driver_no_id.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); EXPECT_CALL(*device_container, load_drivers()); EXPECT_FALSE(device_container->load_drivers()); } TEST_F(DeviceContainerTest, test_load_driver_bad_no_driver) { std::string file_name("bus_configs/bad_driver_no_driver.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); EXPECT_CALL(*device_container, load_drivers()); EXPECT_FALSE(device_container->load_drivers()); } TEST_F(DeviceContainerTest, test_load_driver_bad_no_package) { std::string file_name("bus_configs/bad_driver_no_package.yml"); device_container->config_ = std::make_shared(file_name); device_container->config_->init_config(); EXPECT_CALL(*device_container, load_drivers()); EXPECT_FALSE(device_container->load_drivers()); } TEST_F(DeviceContainerTest, test_load_manager_no_lifecycle) { device_container->lifecycle_operation_ = false; EXPECT_CALL(*device_container, load_manager()); EXPECT_TRUE(device_container->load_manager()); EXPECT_FALSE(device_container->lifecycle_manager_); } TEST_F(DeviceContainerTest, test_load_manager_lifecycle) { device_container->lifecycle_operation_ = true; EXPECT_CALL(*device_container, load_manager()); EXPECT_CALL(*device_container, add_node_to_executor(_)); EXPECT_TRUE(device_container->load_manager()); EXPECT_TRUE(device_container->lifecycle_manager_); } TEST_F(DeviceContainerTest, test_configure_good) { device_container->set_parameter(rclcpp::Parameter("can_interface_name", "vcan0")); device_container->set_parameter(rclcpp::Parameter("master_config", "master.dcf")); device_container->set_parameter( rclcpp::Parameter("bus_config", "bus_configs/good_master_and_two_driver.yml")); EXPECT_CALL(*device_container, configure()); EXPECT_NO_THROW(device_container->configure()); } TEST_F(DeviceContainerTest, test_init) { device_container->set_parameter(rclcpp::Parameter("can_interface_name", "vcan0")); device_container->set_parameter(rclcpp::Parameter("master_config", "master.dcf")); device_container->set_parameter( rclcpp::Parameter("bus_config", "bus_configs/good_master_and_two_driver.yml")); EXPECT_CALL(*device_container, configure()).WillOnce(Return()); EXPECT_CALL(*device_container, load_master()).WillOnce(Return(true)); EXPECT_CALL(*device_container, load_drivers()).WillOnce(Return(true)); EXPECT_CALL(*device_container, load_manager()).WillOnce(Return(true)); EXPECT_NO_THROW(device_container->init()); } TEST_F(DeviceContainerTest, test_init_with_fparam) { EXPECT_CALL(*device_container, load_master()).WillOnce(Return(true)); EXPECT_CALL(*device_container, load_drivers()).WillOnce(Return(true)); EXPECT_CALL(*device_container, load_manager()).WillOnce(Return(true)); EXPECT_NO_THROW(device_container->init( "vcan0", "master.dcf", "bus_configs/good_master_and_two_driver.yml", "")); } // TEST(ComponentLoad, test_device_container_configure) // { // rclcpp::init(0, nullptr); // auto exec = std::make_shared(); // auto device_container = std::make_shared(exec); // exec->add_node(device_container); // device_container->set_parameter(Parameter("bus_config", "bus.yml")); // device_container->set_parameter(Parameter("master_config", "master.dcf")); // device_container->set_parameter(Parameter("can_interface_name", "can0")); // exec->spin_some(100ms); // device_container->configure(); // auto time_now = std::chrono::steady_clock::now(); // auto time_future = time_now + 100ms; // while (time_future > std::chrono::steady_clock::now()) // { // exec->spin_some(100ms); // } // rclcpp::shutdown(); // } // TEST(ComponentLoad, test_load_master) // { // rclcpp::init(0, nullptr); // auto exec = std::make_shared(); // auto device_container = std::make_shared(exec); // exec->add_node(device_container); // device_container->set_parameter(Parameter("bus_config", "bus.yml")); // device_container->set_parameter(Parameter("master_config", "master.dcf")); // device_container->set_parameter(Parameter("can_interface_name", "vcan0")); // exec->spin_some(100ms); // device_container->configure(); // device_container->load_master(); // std::thread spinner( // [exec]() // { // exec->spin(); // RCLCPP_INFO(rclcpp::get_logger("test"), "Executor done."); // }); // std::this_thread::sleep_for(500ms); // device_container->shutdown(); // rclcpp::shutdown(); // spinner.join(); // RCLCPP_INFO(rclcpp::get_logger("test"), "rclcpp::shutdown"); // } // TEST(ComponentLoad, test_load_component_2) // { // rclcpp::init(0, nullptr); // auto exec = std::make_shared(); // auto device_container = std::make_shared(exec); // exec->add_node(device_container); // device_container->set_parameter(Parameter("bus_config", "bus.yml")); // device_container->set_parameter(Parameter("master_config", "master.dcf")); // device_container->set_parameter(Parameter("can_interface_name", "vcan0")); // std::thread spinner ( // [exec](){ // exec->spin(); // RCLCPP_INFO(rclcpp::get_logger("test"), "Executor done."); // } // ); // device_container->configure(); // device_container->load_master(); // EXPECT_THROW(device_container->load_drivers(), std::system_error); // std::this_thread::sleep_for(500ms); // device_container->shutdown(); // rclcpp::shutdown(); // spinner.join(); // } ================================================ FILE: canopen_core/test/test_errors.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/device_container_error.hpp" #include "canopen_core/driver_error.hpp" #include "canopen_core/master_error.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; TEST(DriverErrorTest, test_what) { DriverException test_obj("hello"); EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); } TEST(MasterErrorTest, test_what) { MasterException test_obj("hello"); EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); } TEST(DeviceContainerErrorTest, test_what) { DeviceContainerException test_obj("hello"); EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); } ================================================ FILE: canopen_core/test/test_lifecycle_canopen_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/driver_node.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenDriverInterface { public: MOCK_METHOD( void, set_master, (std::shared_ptr exec, std::shared_ptr master), (override)); MOCK_METHOD(void, demand_set_master, (), (override)); MOCK_METHOD(void, init, (), (override)); MOCK_METHOD(void, configure, (), (override)); MOCK_METHOD(void, activate, (), (override)); MOCK_METHOD(void, deactivate, (), (override)); MOCK_METHOD(void, cleanup, (), (override)); MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(void, add_to_master, (), (override)); MOCK_METHOD(void, remove_from_master, (), (override)); }; class MockCanopenMaster : public LifecycleCanopenDriver { friend class CanopenDriverTest; FRIEND_TEST(CanopenDriverTest, test_init); }; class CanopenDriverTest : public testing::Test { public: std::shared_ptr canopen_driver; std::shared_ptr node_canopen_driver; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "SetUp"); rclcpp::init(0, nullptr); canopen_driver = std::make_shared(); node_canopen_driver = std::make_shared(); canopen_driver->node_canopen_driver_ = std::static_pointer_cast( node_canopen_driver); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(CanopenDriverTest, test_init) { EXPECT_CALL(*node_canopen_driver, init()).Times(1); canopen_driver->init(); } TEST_F(CanopenDriverTest, test_set_master) { std::shared_ptr exec; std::shared_ptr master; EXPECT_CALL(*node_canopen_driver, set_master(_, _)).Times(1); EXPECT_NO_THROW(canopen_driver->set_master(exec, master)); } TEST_F(CanopenDriverTest, test_get_node_base_interface) { auto base_iface = canopen_driver->get_node_base_interface(); EXPECT_TRUE(base_iface); } TEST_F(CanopenDriverTest, test_shutdown) { EXPECT_CALL(*node_canopen_driver, shutdown()); EXPECT_NO_THROW(canopen_driver->shutdown()); } TEST_F(CanopenDriverTest, test_is_lifecycle) { EXPECT_TRUE(canopen_driver->is_lifecycle()); } TEST_F(CanopenDriverTest, test_get_node_canopen_driver_interface) { EXPECT_TRUE(canopen_driver->get_node_canopen_driver_interface() == node_canopen_driver); } TEST_F(CanopenDriverTest, test_on_configure) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_driver, configure()); EXPECT_CALL(*node_canopen_driver, demand_set_master()); canopen_driver->on_configure(state); } TEST_F(CanopenDriverTest, test_on_activate) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_driver, activate()); canopen_driver->on_activate(state); } TEST_F(CanopenDriverTest, test_on_deactivate) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_driver, deactivate()); canopen_driver->on_deactivate(state); } TEST_F(CanopenDriverTest, test_on_cleanup) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_driver, cleanup()); canopen_driver->on_cleanup(state); } TEST_F(CanopenDriverTest, test_on_shutdown) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_driver, shutdown()); canopen_driver->on_shutdown(state); } ================================================ FILE: canopen_core/test/test_lifecycle_canopen_master.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/master_node.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenMasterInterface { public: MOCK_METHOD(void, init, (), (override)); MOCK_METHOD(void, configure, (), (override)); MOCK_METHOD(void, activate, (), (override)); MOCK_METHOD(void, deactivate, (), (override)); MOCK_METHOD(void, cleanup, (), (override)); MOCK_METHOD(void, shutdown, (), (override)); MOCK_METHOD(std::shared_ptr, get_master, (), (override)); MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); }; class MockCanopenMaster : public LifecycleCanopenMaster { friend class CanopenMasterTest; FRIEND_TEST(CanopenMasterTest, test_init); }; class CanopenMasterTest : public testing::Test { public: std::shared_ptr canopen_master; std::shared_ptr node_canopen_master; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); canopen_master = std::make_shared(); node_canopen_master = std::make_shared(); canopen_master->node_canopen_master_ = std::static_pointer_cast( node_canopen_master); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(CanopenMasterTest, test_init) { EXPECT_CALL(*node_canopen_master, init()).Times(1); canopen_master->init(); } TEST_F(CanopenMasterTest, test_get_node_base_interface) { auto base_iface = canopen_master->get_node_base_interface(); EXPECT_TRUE(base_iface); } TEST_F(CanopenMasterTest, test_shutdown) { EXPECT_CALL(*node_canopen_master, shutdown()); EXPECT_NO_THROW(canopen_master->shutdown()); } TEST_F(CanopenMasterTest, test_is_lifecycle) { EXPECT_TRUE(canopen_master->is_lifecycle()); } TEST_F(CanopenMasterTest, test_get_master) { EXPECT_CALL(*node_canopen_master, get_master()); EXPECT_NO_THROW(canopen_master->get_master()); } TEST_F(CanopenMasterTest, test_get_executor) { EXPECT_CALL(*node_canopen_master, get_executor()); EXPECT_NO_THROW(canopen_master->get_executor()); } TEST_F(CanopenMasterTest, test_on_configure) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_master, configure()); canopen_master->on_configure(state); } TEST_F(CanopenMasterTest, test_on_activate) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_master, activate()); canopen_master->on_activate(state); } TEST_F(CanopenMasterTest, test_on_deactivate) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_master, deactivate()); canopen_master->on_deactivate(state); } TEST_F(CanopenMasterTest, test_on_cleanup) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_master, cleanup()); canopen_master->on_cleanup(state); } TEST_F(CanopenMasterTest, test_on_shutdown) { rclcpp_lifecycle::State state; EXPECT_CALL(*node_canopen_master, shutdown()); canopen_master->on_shutdown(state); } ================================================ FILE: canopen_core/test/test_lifecycle_manager.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "canopen_core/lifecycle_manager.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace std::chrono_literals; using namespace ros2_canopen; using namespace testing; class MockLifecycleManager : public LifecycleManager { public: FRIEND_TEST(LifecycleManagerTest, test_constructor); FRIEND_TEST(LifecycleManagerTest, test_init); FRIEND_TEST(LifecycleManagerTest, test_bring_up_master); FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_false_initial_state); FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_failed_activate); FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_failed_configure); FRIEND_TEST(LifecycleManagerTest, test_bring_down_master); FRIEND_TEST(LifecycleManagerTest, test_bring_down_master_false_state); FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver); FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_false_state_master); FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_false_state_driver); FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_fail_configure); FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_fail_activate); FRIEND_TEST(LifecycleManagerTest, test_bring_down_driver); FRIEND_TEST(LifecycleManagerTest, test_bring_down_driver_failed); FRIEND_TEST(LifecycleManagerTest, test_bring_up_all); FRIEND_TEST(LifecycleManagerTest, test_bring_up_all_fail_master); FRIEND_TEST(LifecycleManagerTest, test_bring_up_all_fail_driver); FRIEND_TEST(LifecycleManagerTest, test_bring_down_all); FRIEND_TEST(LifecycleManagerTest, test_bring_down_all_fail_master); FRIEND_TEST(LifecycleManagerTest, test_bring_down_all_fail_driver); FRIEND_TEST(LifecycleManagerTest, test_on_configure); FRIEND_TEST(LifecycleManagerTest, test_on_configure_fail); FRIEND_TEST(LifecycleManagerTest, test_on_activate); FRIEND_TEST(LifecycleManagerTest, test_on_activate_fail); FRIEND_TEST(LifecycleManagerTest, test_on_deactivate); FRIEND_TEST(LifecycleManagerTest, test_on_deactivate_fail); FRIEND_TEST(LifecycleManagerTest, test_on_cleanup); FRIEND_TEST(LifecycleManagerTest, test_on_shutdown); MockLifecycleManager() : LifecycleManager(rclcpp::NodeOptions()) {} MOCK_METHOD(unsigned int, get_state, (uint8_t node_id, std::chrono::seconds time_out)); MOCK_METHOD( bool, change_state, (uint8_t node_id, uint8_t transition, std::chrono::seconds time_out)); MOCK_METHOD(bool, bring_up_master, (), (override)); MOCK_METHOD(bool, bring_down_master, (), (override)); MOCK_METHOD(bool, bring_up_driver, (std::string device_name), (override)); MOCK_METHOD(bool, bring_down_driver, (std::string device_name), (override)); MOCK_METHOD(bool, load_from_config, (), (override)); MOCK_METHOD(bool, bring_up_all, (), (override)); MOCK_METHOD(bool, bring_down_all, (), (override)); void DelegateToBase() { ON_CALL(*this, get_state) .WillByDefault( [this](uint8_t node_id, std::chrono::seconds time_out) -> unsigned int { return LifecycleManager::get_state(node_id, time_out); }); ON_CALL(*this, change_state) .WillByDefault( [this](uint8_t node_id, uint8_t transition, std::chrono::seconds time_out) -> unsigned int { return LifecycleManager::change_state(node_id, transition, time_out); }); ON_CALL(*this, bring_up_master) .WillByDefault([this]() -> bool { return LifecycleManager::bring_up_master(); }); ON_CALL(*this, bring_down_master) .WillByDefault([this]() -> bool { return LifecycleManager::bring_down_master(); }); ON_CALL(*this, bring_up_driver) .WillByDefault( [this](std::string device_name) -> bool { return LifecycleManager::bring_up_driver(device_name); }); ON_CALL(*this, bring_down_driver) .WillByDefault( [this](std::string device_name) -> bool { return LifecycleManager::bring_down_driver(device_name); }); ON_CALL(*this, load_from_config) .WillByDefault([this]() -> bool { return LifecycleManager::load_from_config(); }); ON_CALL(*this, bring_up_all) .WillByDefault([this]() -> bool { return LifecycleManager::bring_up_all(); }); ON_CALL(*this, bring_down_all) .WillByDefault([this]() -> bool { return LifecycleManager::bring_down_all(); }); } }; class LifecycleManagerTest : public testing::Test { public: std::shared_ptr lifecycle_manager; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); lifecycle_manager = std::make_shared(); lifecycle_manager->DelegateToBase(); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } void SetUpConfigurationManager() { std::string file_name("bus_configs/good_master_and_two_driver.yml"); auto cmanager = std::make_shared(file_name); cmanager->init_config(); lifecycle_manager->init(cmanager); EXPECT_CALL(*lifecycle_manager, load_from_config()); lifecycle_manager->load_from_config(); } }; TEST_F(LifecycleManagerTest, test_constructor) { EXPECT_TRUE(lifecycle_manager->has_parameter("container_name")); EXPECT_TRUE(lifecycle_manager->cbg_clients); } TEST_F(LifecycleManagerTest, test_init) { std::string file_name("bus_configs/good_master_and_two_driver.yml"); auto cmanager = std::make_shared(file_name); cmanager->init_config(); lifecycle_manager->init(cmanager); EXPECT_TRUE(lifecycle_manager->config_); } TEST_F(LifecycleManagerTest, test_bring_up_master) { EXPECT_CALL(*lifecycle_manager, bring_up_master()); EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); EXPECT_TRUE(lifecycle_manager->bring_up_master()); } TEST_F(LifecycleManagerTest, test_bring_up_master_false_initial_state) { EXPECT_CALL(*lifecycle_manager, bring_up_master()); EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(0U)); EXPECT_FALSE(lifecycle_manager->bring_up_master()); } TEST_F(LifecycleManagerTest, test_bring_up_master_failed_configure) { EXPECT_CALL(*lifecycle_manager, bring_up_master()); EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(1).WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_master()); } TEST_F(LifecycleManagerTest, test_bring_up_master_failed_activate) { EXPECT_CALL(*lifecycle_manager, bring_up_master()); EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) .Times(2) .WillOnce(Return(true)) .WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_master()); } TEST_F(LifecycleManagerTest, test_bring_down_master) { EXPECT_CALL(*lifecycle_manager, bring_down_master()); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(1) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_TRUE(lifecycle_manager->bring_down_master()); } TEST_F(LifecycleManagerTest, test_bring_down_master_false_state) { EXPECT_CALL(*lifecycle_manager, bring_down_master()); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(1) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); EXPECT_FALSE(lifecycle_manager->bring_down_master()); } TEST_F(LifecycleManagerTest, test_bring_up_driver) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(2) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); EXPECT_TRUE(lifecycle_manager->bring_up_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_up_driver_false_state_master) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(1) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_up_driver_false_state_driver) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(2) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_up_driver_fail_configure) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(2) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(1).WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_up_driver_fail_activate) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(2) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) .Times(2) .WillOnce(Return(true)) .WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_down_driver) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) .Times(2) .WillOnce(Return(true)) .WillOnce(Return(true)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(1) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); EXPECT_TRUE(lifecycle_manager->bring_down_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_down_driver_failed) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)); EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) .Times(2) .WillOnce(Return(true)) .WillOnce(Return(true)); EXPECT_CALL(*lifecycle_manager, get_state(_, _)) .Times(1) .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); EXPECT_FALSE(lifecycle_manager->bring_down_driver("proxy_device_2")); } TEST_F(LifecycleManagerTest, test_bring_up_all) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_all); EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(true)); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)).Times(2).WillRepeatedly(Return(true)); EXPECT_TRUE(lifecycle_manager->bring_up_all()); } TEST_F(LifecycleManagerTest, test_bring_up_all_fail_master) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_all); EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_all()); } TEST_F(LifecycleManagerTest, test_bring_up_all_fail_driver) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_up_all); EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(true)); EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)).Times(1).WillRepeatedly(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_up_all()); } TEST_F(LifecycleManagerTest, test_bring_down_all) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_down_all); EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(2).WillRepeatedly(Return(true)); EXPECT_CALL(*lifecycle_manager, bring_down_master()).Times(1).WillOnce(Return(true)); EXPECT_TRUE(lifecycle_manager->bring_down_all()); } TEST_F(LifecycleManagerTest, test_bring_down_all_fail_driver) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_down_all); EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(1).WillRepeatedly(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_down_all()); } TEST_F(LifecycleManagerTest, test_bring_down_all_fail_master) { SetUpConfigurationManager(); EXPECT_CALL(*lifecycle_manager, bring_down_all); EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(2).WillRepeatedly(Return(true)); EXPECT_CALL(*lifecycle_manager, bring_down_master()).Times(1).WillOnce(Return(false)); EXPECT_FALSE(lifecycle_manager->bring_down_all()); } TEST_F(LifecycleManagerTest, test_on_configure) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, load_from_config()).Times(1).WillOnce(Return(true)); EXPECT_EQ( lifecycle_manager->on_configure(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } TEST_F(LifecycleManagerTest, test_on_configure_fail) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, load_from_config()).Times(1).WillOnce(Return(false)); EXPECT_EQ( lifecycle_manager->on_configure(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); } TEST_F(LifecycleManagerTest, test_on_activate) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, bring_up_all()).Times(1).WillOnce(Return(true)); EXPECT_EQ( lifecycle_manager->on_activate(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } TEST_F(LifecycleManagerTest, test_on_activate_fail) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, bring_up_all()).Times(1).WillOnce(Return(false)); EXPECT_EQ( lifecycle_manager->on_activate(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); } TEST_F(LifecycleManagerTest, test_on_deactivate) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, bring_down_all()).Times(1).WillOnce(Return(true)); EXPECT_EQ( lifecycle_manager->on_deactivate(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } TEST_F(LifecycleManagerTest, test_on_deactivate_fail) { rclcpp_lifecycle::State state; EXPECT_CALL(*lifecycle_manager, bring_down_all()).Times(1).WillOnce(Return(false)); EXPECT_EQ( lifecycle_manager->on_deactivate(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); } TEST_F(LifecycleManagerTest, test_on_cleanup) { rclcpp_lifecycle::State state; EXPECT_EQ( lifecycle_manager->on_cleanup(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } TEST_F(LifecycleManagerTest, test_on_shutdown) { rclcpp_lifecycle::State state; EXPECT_EQ( lifecycle_manager->on_shutdown(state), rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); } ================================================ FILE: canopen_core/test/test_node_canopen_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/driver_node.hpp" #include "canopen_core/node_interfaces/node_canopen_driver.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace ros2_canopen; using namespace ros2_canopen::node_interfaces; using namespace testing; template class MockNodeCanopenDriver : public NodeCanopenDriver { public: FRIEND_TEST(NodeCanopenDriverTest, test_construct); FRIEND_TEST(NodeCanopenDriverTest, test_init); FRIEND_TEST(NodeCanopenDriverTest, test_init_fail_configured); FRIEND_TEST(NodeCanopenDriverTest, test_init_fail_activated); FRIEND_TEST(NodeCanopenDriverTest, test_set_master); FRIEND_TEST(NodeCanopenDriverTest, test_configure); FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_not_initialsed); FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_configured); FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_activated); FRIEND_TEST(NodeCanopenDriverTest, test_activate); FRIEND_TEST(NodeCanopenDriverTest, test_activate_failures); FRIEND_TEST(NodeCanopenDriverTest, test_deactivate); FRIEND_TEST(NodeCanopenDriverTest, test_deactivate_failures); FRIEND_TEST(NodeCanopenDriverTest, test_cleanup); FRIEND_TEST(NodeCanopenDriverTest, test_cleanup_failures); FRIEND_TEST(NodeCanopenDriverTest, test_shutdown); MockNodeCanopenDriver(rclcpp::Node * node) : NodeCanopenDriver(node) {} MOCK_METHOD0_T(add_to_master, void()); MOCK_METHOD0_T(remove_from_master, void()); void DelegateToBase() { // ON_CALL(*this, load_component) // .WillByDefault( // [this]( // std::string & package_name, std::string & driver_name, uint16_t node_id, // std::string & node_name, std::vector & params) -> bool // { return DeviceContainer::load_component(package_name, driver_name, node_id, // node_name, params); }); } }; class NodeCanopenDriverTest : public testing::Test { public: std::shared_ptr> node_canopen_driver; rclcpp::Node::SharedPtr node; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); node = std::make_shared("driver"); node_canopen_driver = std::make_shared>(node.get()); node_canopen_driver->DelegateToBase(); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(NodeCanopenDriverTest, test_construct) { EXPECT_EQ(node.get(), node_canopen_driver->node_); } TEST_F(NodeCanopenDriverTest, test_init) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); node_canopen_driver->init(); EXPECT_TRUE(node_canopen_driver->initialised_.load()); EXPECT_TRUE(node->has_parameter("container_name")); EXPECT_TRUE(node->has_parameter("node_id")); EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); EXPECT_TRUE(node->has_parameter("config")); } TEST_F(NodeCanopenDriverTest, test_init_fail_configured) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->init(), DriverException); EXPECT_FALSE(node_canopen_driver->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenDriverTest, test_init_fail_activated) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->init(), DriverException); EXPECT_FALSE(node_canopen_driver->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenDriverTest, test_set_master) { std::shared_ptr exec; std::shared_ptr master; node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); node_canopen_driver->set_master(exec, master); EXPECT_TRUE(node_canopen_driver->master_set_.load()); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->set_master(exec, master), DriverException); node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->set_master(exec, master), DriverException); } TEST_F(NodeCanopenDriverTest, test_configure) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); node_canopen_driver->init(); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " "\"simple.eds\"\ndcf_path: \"\"\n")); std::cout << node->get_parameter("config").as_string() << std::endl; node_canopen_driver->configure(); EXPECT_TRUE(node_canopen_driver->configured_.load()); } TEST_F(NodeCanopenDriverTest, test_configure_fail_not_initialsed) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->configure(), DriverException); EXPECT_FALSE(node_canopen_driver->configured_.load()); } TEST_F(NodeCanopenDriverTest, test_configure_fail_configured) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); node_canopen_driver->init(); node_canopen_driver->configured_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_driver->configure(), DriverException); } TEST_F(NodeCanopenDriverTest, test_configure_fail_activated) { node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); node_canopen_driver->init(); node_canopen_driver->activated_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_driver->configure(), DriverException); EXPECT_FALSE(node_canopen_driver->configured_.load()); } TEST_F(NodeCanopenDriverTest, test_activate) { node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_CALL(*node_canopen_driver, add_to_master()).Times(1).WillOnce(Return()); node_canopen_driver->activate(); EXPECT_TRUE(node_canopen_driver->activated_.load()); } TEST_F(NodeCanopenDriverTest, test_activate_failures) { node_canopen_driver->initialised_.store(false); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->activate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(false); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->activate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->activate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->activate(), DriverException); } TEST_F(NodeCanopenDriverTest, test_deactivate) { node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_CALL(*node_canopen_driver, remove_from_master()).Times(1).WillOnce(Return()); node_canopen_driver->deactivate(); EXPECT_FALSE(node_canopen_driver->activated_.load()); } TEST_F(NodeCanopenDriverTest, test_deactivate_failures) { node_canopen_driver->initialised_.store(false); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(false); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->master_set_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); } TEST_F(NodeCanopenDriverTest, test_cleanup) { node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); node_canopen_driver->cleanup(); EXPECT_FALSE(node_canopen_driver->configured_.load()); } TEST_F(NodeCanopenDriverTest, test_cleanup_failures) { node_canopen_driver->initialised_.store(false); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); } TEST_F(NodeCanopenDriverTest, test_shutdown) { node_canopen_driver->master_set_.store(true); node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(true); EXPECT_CALL(*node_canopen_driver, remove_from_master()).Times(1).WillOnce(Return()); EXPECT_NO_THROW(node_canopen_driver->shutdown()); EXPECT_FALSE(node_canopen_driver->master_set_.load()); EXPECT_FALSE(node_canopen_driver->initialised_.load()); EXPECT_FALSE(node_canopen_driver->configured_.load()); EXPECT_FALSE(node_canopen_driver->activated_.load()); node_canopen_driver->master_set_.store(true); node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(true); node_canopen_driver->activated_.store(false); EXPECT_NO_THROW(node_canopen_driver->shutdown()); EXPECT_FALSE(node_canopen_driver->master_set_.load()); EXPECT_FALSE(node_canopen_driver->initialised_.load()); EXPECT_FALSE(node_canopen_driver->configured_.load()); EXPECT_FALSE(node_canopen_driver->activated_.load()); node_canopen_driver->master_set_.store(true); node_canopen_driver->initialised_.store(true); node_canopen_driver->configured_.store(false); node_canopen_driver->activated_.store(false); EXPECT_NO_THROW(node_canopen_driver->shutdown()); EXPECT_FALSE(node_canopen_driver->master_set_.load()); EXPECT_FALSE(node_canopen_driver->initialised_.load()); EXPECT_FALSE(node_canopen_driver->configured_.load()); EXPECT_FALSE(node_canopen_driver->activated_.load()); } ================================================ FILE: canopen_core/test/test_node_canopen_master.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_core/master_node.hpp" #include "canopen_core/node_interfaces/node_canopen_master.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" using namespace ros2_canopen; using namespace ros2_canopen::node_interfaces; using namespace testing; template class MockNodeCanopenMaster : public NodeCanopenMaster { public: FRIEND_TEST(NodeCanopenMasterTest, test_construct); FRIEND_TEST(NodeCanopenMasterTest, test_init); FRIEND_TEST(NodeCanopenMasterTest, test_init_fail_configured); FRIEND_TEST(NodeCanopenMasterTest, test_init_fail_activated); FRIEND_TEST(NodeCanopenMasterTest, test_set_master); FRIEND_TEST(NodeCanopenMasterTest, test_configure); FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_not_initialsed); FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_configured); FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_activated); FRIEND_TEST(NodeCanopenMasterTest, test_activate); FRIEND_TEST(NodeCanopenMasterTest, test_activate_failures); FRIEND_TEST(NodeCanopenMasterTest, test_deactivate); FRIEND_TEST(NodeCanopenMasterTest, test_deactivate_failures); FRIEND_TEST(NodeCanopenMasterTest, test_cleanup); FRIEND_TEST(NodeCanopenMasterTest, test_cleanup_failures); FRIEND_TEST(NodeCanopenMasterTest, test_shutdown); FRIEND_TEST(NodeCanopenMasterTest, test_get_master); FRIEND_TEST(NodeCanopenMasterTest, test_get_executor); FRIEND_TEST(NodeCanopenLMasterTest, test_construct); FRIEND_TEST(NodeCanopenLMasterTest, test_init); FRIEND_TEST(NodeCanopenLMasterTest, test_init_fail_configured); FRIEND_TEST(NodeCanopenLMasterTest, test_init_fail_activated); FRIEND_TEST(NodeCanopenLMasterTest, test_set_master); FRIEND_TEST(NodeCanopenLMasterTest, test_configure); FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_not_initialsed); FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_configured); FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_activated); FRIEND_TEST(NodeCanopenLMasterTest, test_activate); FRIEND_TEST(NodeCanopenLMasterTest, test_activate_failures); FRIEND_TEST(NodeCanopenLMasterTest, test_deactivate); FRIEND_TEST(NodeCanopenLMasterTest, test_deactivate_failures); FRIEND_TEST(NodeCanopenLMasterTest, test_cleanup); FRIEND_TEST(NodeCanopenLMasterTest, test_cleanup_failures); FRIEND_TEST(NodeCanopenLMasterTest, test_shutdown); FRIEND_TEST(NodeCanopenLMasterTest, test_get_master); FRIEND_TEST(NodeCanopenLMasterTest, test_get_executor); MockNodeCanopenMaster(NODETYPE * node) : NodeCanopenMaster(node) {} void DelegateToBase() { // ON_CALL(*this, load_component) // .WillByDefault( // [this]( // std::string & package_name, std::string & driver_name, uint16_t node_id, // std::string & node_name, std::vector & params) -> bool // { return DeviceContainer::load_component(package_name, driver_name, node_id, // node_name, params); }); } }; class NodeCanopenMasterTest : public testing::Test { public: std::shared_ptr> node_canopen_master; rclcpp::Node::SharedPtr node; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); node = std::make_shared("driver"); node_canopen_master = std::make_shared>(node.get()); node_canopen_master->DelegateToBase(); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(NodeCanopenMasterTest, test_construct) { EXPECT_EQ(node.get(), node_canopen_master->node_); } TEST_F(NodeCanopenMasterTest, test_init) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); EXPECT_TRUE(node_canopen_master->initialised_.load()); EXPECT_TRUE(node->has_parameter("container_name")); EXPECT_TRUE(node->has_parameter("node_id")); EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); EXPECT_TRUE(node->has_parameter("config")); } TEST_F(NodeCanopenMasterTest, test_init_fail_configured) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->init(), MasterException); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenMasterTest, test_init_fail_activated) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->init(), MasterException); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenMasterTest, test_configure) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenMaster\"\npackage:\"canopen_core\"\n")); node_canopen_master->configure(); EXPECT_TRUE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenMasterTest, test_configure_fail_not_initialsed) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->configure(), MasterException); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenMasterTest, test_configure_fail_configured) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node_canopen_master->configured_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_master->configure(), MasterException); } TEST_F(NodeCanopenMasterTest, test_configure_fail_activated) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node_canopen_master->activated_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_master->configure(), MasterException); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenMasterTest, test_activate_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->activate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->activate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->activate(), MasterException); } TEST_F(NodeCanopenMasterTest, test_deactivate_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); } TEST_F(NodeCanopenMasterTest, test_cleanup) { node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); node_canopen_master->cleanup(); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenMasterTest, test_cleanup_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); } TEST_F(NodeCanopenMasterTest, test_shutdown) { node_canopen_master->master_set_.store(true); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_NO_THROW(node_canopen_master->shutdown()); EXPECT_FALSE(node_canopen_master->master_set_.load()); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node_canopen_master->configured_.load()); EXPECT_FALSE(node_canopen_master->activated_.load()); } TEST_F(NodeCanopenMasterTest, test_get_master) { node_canopen_master->master_set_.store(true); EXPECT_NO_THROW(node_canopen_master->get_master()); node_canopen_master->master_set_.store(false); EXPECT_ANY_THROW(node_canopen_master->get_master()); } TEST_F(NodeCanopenMasterTest, test_get_executor) { node_canopen_master->master_set_.store(true); EXPECT_NO_THROW(node_canopen_master->get_executor()); node_canopen_master->master_set_.store(false); EXPECT_ANY_THROW(node_canopen_master->get_executor()); } class NodeCanopenLMasterTest : public testing::Test { public: std::shared_ptr> node_canopen_master; rclcpp_lifecycle::LifecycleNode::SharedPtr node; void SetUp() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); rclcpp::init(0, nullptr); node = std::make_shared("driver"); node_canopen_master = std::make_shared>(node.get()); node_canopen_master->DelegateToBase(); } void TearDown() override { RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); rclcpp::shutdown(); } }; TEST_F(NodeCanopenLMasterTest, test_construct) { EXPECT_EQ(node.get(), node_canopen_master->node_); } TEST_F(NodeCanopenLMasterTest, test_init) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); EXPECT_TRUE(node_canopen_master->initialised_.load()); EXPECT_TRUE(node->has_parameter("container_name")); EXPECT_TRUE(node->has_parameter("node_id")); EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); EXPECT_TRUE(node->has_parameter("config")); } TEST_F(NodeCanopenLMasterTest, test_init_fail_configured) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->init(), MasterException); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenLMasterTest, test_init_fail_activated) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->init(), MasterException); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node->has_parameter("container_name")); EXPECT_FALSE(node->has_parameter("node_id")); EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); EXPECT_FALSE(node->has_parameter("config")); } TEST_F(NodeCanopenLMasterTest, test_configure) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenMaster\"\npackage:\"canopen_core\"\n")); node_canopen_master->configure(); EXPECT_TRUE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenLMasterTest, test_configure_fail_not_initialsed) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->configure(), MasterException); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenLMasterTest, test_configure_fail_configured) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node_canopen_master->configured_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_master->configure(), MasterException); } TEST_F(NodeCanopenLMasterTest, test_configure_fail_activated) { node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); node_canopen_master->init(); node_canopen_master->activated_.store(true); node->set_parameter(rclcpp::Parameter( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); EXPECT_THROW(node_canopen_master->configure(), MasterException); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenLMasterTest, test_activate_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->activate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->activate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->activate(), MasterException); } TEST_F(NodeCanopenLMasterTest, test_deactivate_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->deactivate(), MasterException); } TEST_F(NodeCanopenLMasterTest, test_cleanup) { node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); node_canopen_master->cleanup(); EXPECT_FALSE(node_canopen_master->configured_.load()); } TEST_F(NodeCanopenLMasterTest, test_cleanup_failures) { node_canopen_master->initialised_.store(false); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(true); node_canopen_master->activated_.store(true); EXPECT_THROW(node_canopen_master->cleanup(), MasterException); } TEST_F(NodeCanopenLMasterTest, test_shutdown) { node_canopen_master->master_set_.store(true); node_canopen_master->initialised_.store(true); node_canopen_master->configured_.store(false); node_canopen_master->activated_.store(false); EXPECT_NO_THROW(node_canopen_master->shutdown()); EXPECT_FALSE(node_canopen_master->master_set_.load()); EXPECT_FALSE(node_canopen_master->initialised_.load()); EXPECT_FALSE(node_canopen_master->configured_.load()); EXPECT_FALSE(node_canopen_master->activated_.load()); } TEST_F(NodeCanopenLMasterTest, test_get_master) { node_canopen_master->master_set_.store(true); EXPECT_NO_THROW(node_canopen_master->get_master()); node_canopen_master->master_set_.store(false); EXPECT_ANY_THROW(node_canopen_master->get_master()); } TEST_F(NodeCanopenLMasterTest, test_get_executor) { node_canopen_master->master_set_.store(true); EXPECT_NO_THROW(node_canopen_master->get_executor()); node_canopen_master->master_set_.store(false); EXPECT_ANY_THROW(node_canopen_master->get_executor()); } ================================================ FILE: canopen_fake_slaves/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * `#400 `_: Update controllers for ROS Rolling API changes and fix fake slave type handling * Refactor GetValue method to use std::type_index for type comparisons * `motion_generator` as shared library (`#295 `_) * Enhance SimpleSlave class with vendor ID parsing and adjust bus configuration timeouts * Multiple SDO types for slaves (`#278 `_) Co-authored-by: Kurtis Thrush Co-authored-by: Christoph Hellmann Santos * Contributors: Christoph Hellmann Santos, Patrick Roncagliolo, Vishnuprasad Prachandabhanu 0.2.9 (2024-04-16) ------------------ 0.3.1 (2025-06-23) ------------------ * Add boot timeout and retry * Add suported modes to `canopen_fake_slaves` README (`#337 `_) * Contributors: Gerry Salinas, Patrick Roncagliolo, Vishnuprasad Prachandabhanu 0.3.0 (2024-12-12) ------------------ * fix loop timer for run_velocity_mode * Fix clang format * Add comments for the fake slave function * Fix the thread issue. * Apply suggestions from code review Co-authored-by: Dr. Denis * Working version. * Periodic messages sent, but not received properly. * Working logic. Still have to work on the edf file. * Put the periodic messages in OnWrite * Extend fake_slaves to publish messages via rpdo 0.2.12 (2024-04-22) ------------------- * Merge pull request `#265 `_ from kurtist123/feature/expose-fake-slave-includes build: export include directories * build: export include directories * 0.2.9 * forthcoming * Contributors: Kurtis Thrush, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ * Add fake profile velocity (`#230 `_) * Add simple sequence homing emulation * Add fake velocity mode * Formatting * Add simple sequence homing emulation (`#229 `_) * Contributors: Christoph Hellmann Santos 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix fake slave for PDOs * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, James Ward, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_fake_slaves/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_fake_slaves) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(lely_core_libraries REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) add_library( motion_generator SHARED "src/motion_generator.cpp" ) target_compile_features(motion_generator PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(motion_generator PUBLIC $ $) add_executable( basic_slave_node "src/basic_slave.cpp" ) target_compile_features(basic_slave_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(basic_slave_node PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(basic_slave_node PUBLIC ${lely_core_libraries_LIBRARIES} ${lifecycle_msgs_TARGETS} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) add_executable( cia402_slave_node "src/cia402_slave.cpp" ) target_compile_features(cia402_slave_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(cia402_slave_node PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(cia402_slave_node PUBLIC ${lely_core_libraries_LIBRARIES} ${lifecycle_msgs_TARGETS} motion_generator rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) install(TARGETS motion_generator DESTINATION lib/${PROJECT_NAME}) install(TARGETS basic_slave_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS cia402_slave_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/ ) install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config/ ) install(DIRECTORY include/ DESTINATION include ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() ament_export_include_directories( include ) ament_export_dependencies( lely_core_libraries lifecycle_msgs rclcpp rclcpp_lifecycle ) ament_package() ================================================ FILE: canopen_fake_slaves/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_fake_slaves/Readme.md ================================================ # CANopen Mock Slaves ## Motion Controller Slaves Supported modes: * Cyclic Position * Profiled Position (Thanks to motion generator https://github.com/EFeru/MotionGenerator) * Interpolated Position * Homing * Profiled Velocity ================================================ FILE: canopen_fake_slaves/config/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0xC0000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0xC0000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_fake_slaves/config/simple_slave.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=11 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1A00 [ManufacturerObjects] SupportedObjects=2 1=0x4000 2=0x4001 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 ================================================ FILE: canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef SLAVE_HPP #define SLAVE_HPP #include #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace ros2_canopen { class BaseSlave : public rclcpp_lifecycle::LifecycleNode { public: explicit BaseSlave(const std::string & node_name, bool intra_process_comms = false) : rclcpp_lifecycle::LifecycleNode( node_name, rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) { this->declare_parameter("node_id", 2); this->declare_parameter("slave_config", "slave.eds"); this->declare_parameter("can_interface_name", "vcan0"); this->activated.store(false); } virtual ~BaseSlave() { if (this->run_thread.joinable()) { run_thread.join(); } } virtual void run() = 0; protected: std::thread run_thread; int node_id_; std::string slave_config_; std::string can_interface_name_; std::atomic activated; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State &) { this->activated.store(false); RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State &) { this->activated.store(true); get_parameter("node_id", node_id_); get_parameter("slave_config", slave_config_); get_parameter("can_interface_name", can_interface_name_); run_thread = std::thread(std::bind(&BaseSlave::run, this)); RCLCPP_INFO(this->get_logger(), "Reaching active state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State &) { this->activated.store(false); RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); if (run_thread.joinable()) { run_thread.join(); } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State &) { this->activated.store(false); RCLCPP_INFO(this->get_logger(), "Reaching unconfigured state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "Shutdown"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef BASIC_SLAVE_HPP #define BASIC_SLAVE_HPP #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "canopen_fake_slaves/base_slave.hpp" #include "lifecycle_msgs/msg/state.hpp" using namespace lely; using namespace std::chrono_literals; namespace ros2_canopen { class SimpleSlave : public canopen::BasicSlave { public: SimpleSlave( io::TimerBase & timer, io::CanChannelBase & chan, const std::string & slave_config, const std::string & slave_bin, uint8_t node_id) : canopen::BasicSlave(timer, chan, slave_config, slave_bin, node_id) { const auto vendor_id = parse_vendor_id(slave_config); if (vendor_id != 0) { try { this->OnRead( 0x1018, 0x01, [vendor_id](uint16_t, uint8_t, uint32_t & value) -> std::error_code { value = vendor_id; return {}; }); } catch (const lely::canopen::SdoError &) { // Leave the dictionary untouched if the callback cannot be registered. } } } SimpleSlave( io::TimerBase & timer, io::CanChannelBase & chan, const std::string & slave_config, uint8_t node_id) : SimpleSlave(timer, chan, slave_config, "", node_id) { } ~SimpleSlave() { if (message_thread.joinable()) { message_thread.join(); } } protected: std::thread message_thread; static uint32_t parse_vendor_id(const std::string & slave_config) { std::ifstream stream(slave_config); if (!stream.is_open()) { return 0; } std::string line; while (std::getline(stream, line)) { auto pos = line.find("VendorNumber="); if (pos == std::string::npos) { continue; } auto value_part = line.substr(pos + std::string("VendorNumber=").length()); try { return static_cast(std::stoul(value_part, nullptr, 0)); } catch (const std::exception &) { return 0; } } return 0; } /** * @brief This function gets an object value through the typed interface. * Only supports object types that can fit in a 32-bit container. * @param idx The index of the PDO. * @param subidx The subindex of the PDO. * @return value of object stored in a 32-bit container */ uint32_t GetValue(const uint16_t idx, const uint8_t subidx) const noexcept { const std::type_index type((*this)[idx][subidx].Type()); uint32_t value{0}; if (type == std::type_index(typeid(bool))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(int8_t))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(int16_t))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(int32_t))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(float))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(uint8_t))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(uint16_t))) { value = static_cast((*this)[idx][subidx].Get()); } else if (type == std::type_index(typeid(uint32_t))) { value = (*this)[idx][subidx].Get(); } else { value = (*this)[idx][subidx].Get(); } return value; } /** * @brief This function is called when a value is written to the local object dictionary by an SDO * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device * @param idx The index of the PDO. * @param subidx The subindex of the PDO. */ void OnWrite(uint16_t idx, uint8_t subidx) noexcept override { (*this)[0x4001][0] = this->GetValue(idx, subidx); this->TpdoEvent(0); // Publish periodic message if (!message_thread.joinable()) { message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } } /** * @brief This function is attached to a thread and sends periodic messages * via 0x4004 */ void fake_periodic_messages() { // If ros is running, send messages while (rclcpp::ok()) { uint32_t val = 0x1122; (*this)[0x4001][0] = val; // refresh TPDO-mapped UNSIGNED32 without touching config entries this->TpdoEvent(0); // 100 ms sleep - 10 Hz std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } }; class BasicSlave : public BaseSlave { public: explicit BasicSlave(const std::string & node_name, bool intra_process_comms = false) : BaseSlave(node_name, intra_process_comms) { } protected: class ActiveCheckTask : public ev::CoTask { public: ActiveCheckTask(io::Context * ctx, ev::Executor * exec, BasicSlave * slave) : CoTask(*exec) { slave_ = slave; exec_ = exec; ctx_ = ctx; } protected: BasicSlave * slave_; ev::Executor * exec_; io::Context * ctx_; virtual void operator()() noexcept { if (slave_->activated.load()) { } ctx_->shutdown(); } }; void run() override { io::IoGuard io_guard; io::Context ctx; io::Poll poll(ctx); ev::Loop loop(poll.get_poll()); auto exec = loop.get_executor(); io::Timer timer(poll, exec, CLOCK_MONOTONIC); io::CanController ctrl(can_interface_name_.c_str()); io::CanChannel chan(poll, exec); chan.open(ctrl); auto sigset_ = lely::io::SignalSet(poll, exec); // Watch for Ctrl+C or process termination. sigset_.insert(SIGHUP); sigset_.insert(SIGINT); sigset_.insert(SIGTERM); sigset_.submit_wait( [&](int /*signo*/) { // If the signal is raised again, terminate immediately. sigset_.clear(); // Perform a clean shutdown. ctx.shutdown(); }); SimpleSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); slave.Reset(); ActiveCheckTask checktask(&ctx, &exec, this); // timer.submit_wait() RCLCPP_INFO(this->get_logger(), "Created slave for node_id %i.", node_id_); loop.run(); ctx.shutdown(); RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); rclcpp::shutdown(); } }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CIA402_SLAVE_HPP #define CIA402_SLAVE_HPP #include #include #include #include #include #include #include #include #include #include #include #include "canopen_fake_slaves/base_slave.hpp" #include "canopen_fake_slaves/motion_generator.hpp" #include "lifecycle_msgs/msg/state.hpp" using namespace lely; using namespace std::chrono_literals; namespace ros2_canopen { class CIA402MockSlave : public canopen::BasicSlave { public: explicit CIA402MockSlave( io::TimerBase & timer, io::CanChannelBase & chan, const ::std::string & dcf_txt, const ::std::string & dcf_bin = "", uint8_t id = 0xff) : canopen::BasicSlave(timer, chan, dcf_txt, dcf_bin, id) { state.store(InternalState::Not_Ready_To_Switch_On); status_word = 0x0; operation_mode.store(No_Mode); control_cycle_period = 0.01; actual_position = 0.0; } virtual ~CIA402MockSlave() { if (profiled_position_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined profiled_position_mode thread."); profiled_position_mode.join(); } if (cyclic_position_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); cyclic_position_mode.join(); } if (interpolated_position_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } if (homing_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } if (profiled_velocity_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); profiled_velocity_mode.join(); } } protected: enum InternalState { Unknown = 0, Start = 0, Not_Ready_To_Switch_On = 1, Switch_On_Disabled = 2, Ready_To_Switch_On = 3, Switched_On = 4, Operation_Enable = 5, Quick_Stop_Active = 6, Fault_Reaction_Active = 7, Fault = 8, }; enum StatusWord { SW_Ready_To_Switch_On = 0, SW_Switched_On = 1, SW_Operation_enabled = 2, SW_Fault = 3, SW_Voltage_enabled = 4, SW_Quick_stop = 5, SW_Switch_on_disabled = 6, SW_Warning = 7, SW_Manufacturer_specific0 = 8, SW_Remote = 9, SW_Target_reached = 10, SW_Internal_limit = 11, SW_Operation_mode_specific0 = 12, SW_Operation_mode_specific1 = 13, SW_Manufacturer_specific1 = 14, SW_Manufacturer_specific2 = 15 }; enum ControlWord { CW_Switch_On = 0, CW_Enable_Voltage = 1, CW_Quick_Stop = 2, CW_Enable_Operation = 3, CW_Operation_mode_specific0 = 4, CW_Operation_mode_specific1 = 5, CW_Operation_mode_specific2 = 6, CW_Fault_Reset = 7, CW_Halt = 8, CW_Operation_mode_specific3 = 9, // CW_Reserved1=10, CW_Manufacturer_specific0 = 11, CW_Manufacturer_specific1 = 12, CW_Manufacturer_specific2 = 13, CW_Manufacturer_specific3 = 14, CW_Manufacturer_specific4 = 15, }; enum OperationMode { No_Mode = 0, Profiled_Position = 1, Velocity = 2, Profiled_Velocity = 3, Profiled_Torque = 4, Reserved = 5, Homing = 6, Interpolated_Position = 7, Cyclic_Synchronous_Position = 8, Cyclic_Synchronous_Velocity = 9, Cyclic_Synchronous_Torque = 10, }; enum CiaRegister : uint16_t { ControlWord = 0x6040, StatusWord = 0x6041, OperationMode = 0x6060, ModeOfOperationDisplay = 0x6061, ActualPosition = 0x6064, ActualVelocity = 0x606C, TargetPosition = 0x607A, SoftwarePositionLimits = 0x607D, ProfileVelocity = 0x6081, ProfileAcceleration = 0x6083, InterpolationDataRecord = 0x60C1, InterpolationTimePeriod = 0x60C2, PositionOffset = 0x60B0, TargetVelocity = 0x60FF, }; std::atomic is_relative; std::atomic is_running; std::atomic is_halt; std::atomic is_new_set_point; std::atomic operation_mode; std::atomic old_operation_mode; std::mutex w_mutex; uint16_t status_word; uint16_t control_word; std::atomic state; std::thread profiled_position_mode; std::thread profiled_velocity_mode; std::thread cyclic_position_mode; std::thread cyclic_velocity_mode; std::thread interpolated_position_mode; std::thread homing_mode; double cycle_time; std::mutex in_mode_mutex; double actual_position; double actual_speed; double acceleration; double control_cycle_period; void run_profiled_position_mode() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profiled_position_mode"); double profile_speed = static_cast(((uint32_t)(*this)[CiaRegister::ProfileVelocity][0])) / 1000; double profile_accerlation = static_cast(((uint32_t)(*this)[CiaRegister::ProfileAcceleration][0])) / 1000; double actual_position = static_cast(((int32_t)(*this)[CiaRegister::ActualPosition][0])) / 1000.0; double target_position = static_cast(((int32_t)(*this)[CiaRegister::TargetPosition][0])) / 1000.0; double actual_speed = static_cast(((int32_t)(*this)[CiaRegister::ActualVelocity][0])) / 1000.0; RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Profile_Speed %f, Profile Acceleration: %f", profile_speed, profile_accerlation); while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Profiled_Position) && (rclcpp::ok())) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); target_position = static_cast(((int32_t)(*this)[CiaRegister::TargetPosition][0])) / 1000.0; clear_status_bit(SW_Operation_mode_specific0); set_status_bit(SW_Voltage_enabled); set_status_bit(SW_Remote); if (target_position != actual_position) { clear_status_bit(SW_Operation_mode_specific0); clear_status_bit(SW_Target_reached); { std::scoped_lock lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } is_new_set_point.store(false); RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Move from %f to %f", actual_position, target_position); { MotionGenerator gen(profile_accerlation, profile_speed, actual_position); while (!gen.getFinished()) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); actual_position = gen.update(target_position); actual_speed = gen.getVelocity(); (*this)[CiaRegister::ActualPosition][0] = (int32_t)(actual_position * 1000); (*this)[CiaRegister::ActualVelocity][0] = (int32_t)(actual_speed * 1000); } } RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Reached target position %f", actual_position); clear_status_bit(SW_Operation_mode_specific0); set_status_bit(SW_Target_reached); { std::scoped_lock lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } } } } void run_cyclic_position_mode() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_cyclic_position_mode"); int32_t min_pos = (int32_t)(*this)[CiaRegister::SoftwarePositionLimits][1]; int32_t max_pos = (int32_t)(*this)[CiaRegister::SoftwarePositionLimits][2]; uint8_t int_period = (*this)[CiaRegister::InterpolationTimePeriod][1]; int32_t offset = (*this)[CiaRegister::PositionOffset][0]; int8_t index = (*this)[CiaRegister::InterpolationTimePeriod][2]; RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Lower Software Limit: %d", min_pos); RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Upper Software Limit: %d", max_pos); RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Control Cycle: %hhu + 10^%hhd", int_period, index); RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Offset: %d", offset); double cp_min_position = min_pos / 1000; double cp_max_position = max_pos / 1000; double cp_interpolation_period = int_period * std::pow(10.0, index); double cp_offset = (double)(offset / 1000.0); int ccp_millis = (int)(control_cycle_period * std::pow(10.0, 3)); int32_t act_pos; while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Cyclic_Synchronous_Position) && (rclcpp::ok())) { act_pos = (*this)[CiaRegister::TargetPosition][0]; double target_position = (act_pos) / 1000 - cp_offset; // m double position_delta = target_position - actual_position; // m double speed = position_delta / cp_interpolation_period; // m/s double increment = control_cycle_period * speed; // m (*this)[CiaRegister::ActualVelocity][0] = (int32_t)speed * 1000; if ( (target_position < cp_max_position) && (target_position > cp_min_position) && (std::abs(position_delta) > 0.001)) { while ((std::abs(actual_position - target_position) > 0.001) && (rclcpp::ok())) { std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); actual_position += increment; (*this)[CiaRegister::ActualPosition][0] = (int32_t)actual_position * 1000; if (std::abs(actual_position - target_position) < 0.001) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Reached Target %f", target_position); } } } std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); } } void run_interpolated_position_mode() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_interpolated_position_mode"); // Retrieve parameters from the object dictionary double interpolation_period = static_cast((uint8_t)(*this)[CiaRegister::InterpolationTimePeriod][1]); double target_position = static_cast((int32_t)(*this)[CiaRegister::InterpolationDataRecord][1]); // int32_t offset = (*this)[CiaRegister::PositionOffset][0]; // Convert parameters to SI units interpolation_period *= std::pow(10.0, static_cast((int8_t)(*this)[CiaRegister::InterpolationTimePeriod][2])); target_position /= 1000.0; double actual_position = static_cast((int32_t)(*this)[CiaRegister::ActualPosition][0]) / 1000.0; RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Interpolation Period: %f", interpolation_period); RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Target position: %f", target_position); while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Interpolated_Position) && (rclcpp::ok())) { std::this_thread::sleep_for( std::chrono::milliseconds(static_cast(interpolation_period * 1000))); target_position = static_cast((int32_t)(*this)[CiaRegister::InterpolationDataRecord][1]) / 1000.0; if (target_position != actual_position) { double position_delta = target_position - actual_position; double position_increment = position_delta / 20; clear_status_bit(SW_Operation_mode_specific0); clear_status_bit(SW_Target_reached); { std::scoped_lock lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } while ((std::abs(actual_position - target_position) > 0.001) && (rclcpp::ok())) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); actual_position += position_increment; (*this)[CiaRegister::ActualPosition][0] = static_cast(actual_position * 1000); (*this)[CiaRegister::ActualVelocity][0] = static_cast(position_increment * 1000); } actual_position = target_position; RCLCPP_DEBUG(rclcpp::get_logger("cia402_slave"), "Reached target: %f", actual_position); clear_status_bit(SW_Operation_mode_specific0); set_status_bit(SW_Target_reached); { std::lock_guard lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } } } } void run_profile_velocity_mode() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profile_velocity_mode"); double actual_position = static_cast(((int32_t)(*this)[CiaRegister::ActualPosition][0])) / 1000.0; double target_velocity = static_cast(((int32_t)(*this)[CiaRegister::TargetVelocity][0])) / 1000.0; double old_target = target_velocity; double control_cycle_period_d = 0.01; while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Profiled_Velocity) && (rclcpp::ok())) { actual_position = static_cast(((int32_t)(*this)[CiaRegister::ActualPosition][0])) / 1000.0; target_velocity = static_cast(((int32_t)(*this)[CiaRegister::TargetVelocity][0])) / 1000.0; if (old_target != target_velocity) { old_target = target_velocity; RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "New target velocity: %f", target_velocity); } (*this)[CiaRegister::ActualVelocity][0] = (int32_t)(target_velocity * 1000); (*this)[CiaRegister::ActualPosition][0] = (int32_t)((actual_position + target_velocity * control_cycle_period_d) * 1000.0); std::this_thread::sleep_for( std::chrono::milliseconds(((int32_t)(control_cycle_period_d * 1000.0)))); } RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Leaving run_profile_velocity_mode"); } void run_homing_mode() { bool homed = false; while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Homing) && (rclcpp::ok())) { bool start_homing = control_word & CW_Operation_mode_specific0; if (start_homing && !homed) { set_status_bit(SW_Manufacturer_specific1); // Motor active } else { homed = false; continue; } { std::lock_guard lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (start_homing) { clear_status_bit(SW_Manufacturer_specific1); // Motor inactive set_status_bit(SW_Target_reached); // Homing complete set_status_bit(SW_Operation_mode_specific0); // Homing attained (*this)[CiaRegister::StatusWord][0] = status_word; (*this)[CiaRegister::ActualPosition][0] = static_cast(0); (*this)[CiaRegister::ActualVelocity][0] = static_cast(0); homed = true; } } } void set_new_status_word_and_state() { switch (state.load()) { case InternalState::Not_Ready_To_Switch_On: on_not_ready_to_switch_on(); break; case InternalState::Switch_On_Disabled: on_switch_on_disabled(); break; case InternalState::Ready_To_Switch_On: on_ready_to_switch_on(); break; case InternalState::Switched_On: on_switched_on(); break; case InternalState::Operation_Enable: on_operation_enabled(); break; case InternalState::Quick_Stop_Active: on_quickstop_active(); break; case InternalState::Fault_Reaction_Active: break; case InternalState::Fault: RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Fault"); break; default: break; } } void set_status_bit(int bit) { std::scoped_lock lock(w_mutex); status_word |= 1UL << bit; } void clear_status_bit(int bit) { std::scoped_lock lock(w_mutex); status_word &= ~(1UL << bit); } void set_switch_on_disabled() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switch_On_Disabled"); state.store(InternalState::Switch_On_Disabled); clear_status_bit(SW_Ready_To_Switch_On); clear_status_bit(SW_Switched_On); clear_status_bit(SW_Operation_enabled); clear_status_bit(SW_Fault); set_status_bit(SW_Switch_on_disabled); } void set_ready_to_switch_on() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Ready_To_Switch_On"); state.store(InternalState::Ready_To_Switch_On); set_status_bit(SW_Ready_To_Switch_On); clear_status_bit(SW_Switched_On); clear_status_bit(SW_Operation_enabled); clear_status_bit(SW_Fault); set_status_bit(SW_Quick_stop); clear_status_bit(SW_Switch_on_disabled); } void set_switch_on() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched_On"); state.store(InternalState::Switched_On); set_status_bit(SW_Ready_To_Switch_On); set_status_bit(SW_Switched_On); clear_status_bit(SW_Operation_enabled); clear_status_bit(SW_Fault); set_status_bit(SW_Quick_stop); clear_status_bit(SW_Switch_on_disabled); } void set_operation_enabled() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Operation_Enable"); state.store(InternalState::Operation_Enable); set_status_bit(SW_Ready_To_Switch_On); set_status_bit(SW_Switched_On); set_status_bit(SW_Operation_enabled); clear_status_bit(SW_Fault); set_status_bit(SW_Quick_stop); clear_status_bit(SW_Switch_on_disabled); } void set_quick_stop() { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Quick_Stop_Active"); state.store(InternalState::Quick_Stop_Active); set_status_bit(SW_Ready_To_Switch_On); set_status_bit(SW_Switched_On); set_status_bit(SW_Operation_enabled); clear_status_bit(SW_Fault); clear_status_bit(SW_Quick_stop); clear_status_bit(SW_Switch_on_disabled); } void on_not_ready_to_switch_on() { set_switch_on_disabled(); } void on_switch_on_disabled() { if (is_shutdown()) { set_ready_to_switch_on(); } } void on_ready_to_switch_on() { if (is_disable_voltage()) { set_switch_on_disabled(); } if (is_switch_on()) { set_switch_on(); } if (is_faul_reset()) { set_ready_to_switch_on(); } } void on_switched_on() { if (is_disable_voltage()) { set_switch_on_disabled(); } if (is_shutdown()) { set_ready_to_switch_on(); } if (is_enable_operation()) { set_operation_enabled(); } } void on_operation_enabled() { if (is_disable_voltage()) { set_switch_on_disabled(); } if (is_shutdown()) { set_ready_to_switch_on(); } if (is_switch_on()) { set_switch_on(); } if (is_quickstop()) { set_quick_stop(); } { std::scoped_lock lock(w_mutex); is_relative.store(((control_word >> 6) & 1U) == 1U); is_halt.store(((control_word >> 8) & 1U) == 1U); is_new_set_point.store(((control_word >> 4) & 1U) == 1U); } if (old_operation_mode.load() != operation_mode.load()) { // Clear homing-related/ack bits on mode change so new targets are accepted clear_status_bit(SW_Operation_mode_specific0); clear_status_bit(SW_Target_reached); { std::scoped_lock lock(w_mutex); (*this)[0x6041][0] = status_word; this->TpdoEvent(1); } if (profiled_position_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined profiled_position_mode thread."); profiled_position_mode.join(); } if (cyclic_position_mode.joinable()) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); cyclic_position_mode.join(); } if (interpolated_position_mode.joinable()) { RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } if (homing_mode.joinable()) { RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } if (profiled_velocity_mode.joinable()) { RCLCPP_INFO( rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); profiled_velocity_mode.join(); } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { case Cyclic_Synchronous_Position: start_sync_pos_mode(); break; case Profiled_Position: start_profile_pos_mode(); break; case Interpolated_Position: start_interpolated_pos_mode(); break; case Homing: start_homing_mode(); break; case Profiled_Velocity: start_profile_velocity_mode(); break; default: break; } } } void start_sync_pos_mode() { cyclic_position_mode = std::thread(std::bind(&CIA402MockSlave::run_cyclic_position_mode, this)); } void start_profile_pos_mode() { profiled_position_mode = std::thread(std::bind(&CIA402MockSlave::run_profiled_position_mode, this)); } void start_interpolated_pos_mode() { interpolated_position_mode = std::thread(std::bind(&CIA402MockSlave::run_interpolated_position_mode, this)); } void start_homing_mode() { homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); } void start_profile_velocity_mode() { profiled_velocity_mode = std::thread(std::bind(&CIA402MockSlave::run_profile_velocity_mode, this)); } void on_quickstop_active() { if (is_enable_operation()) { set_operation_enabled(); } if (is_disable_voltage()) { set_switch_on_disabled(); } } bool is_shutdown() { std::scoped_lock lock(w_mutex); bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; bool so_unset = ((control_word >> CW_Switch_On) & 1U) == 0U; if (fr_unset && qs_set && ev_set && so_unset) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Shutdown."); return true; } return false; } bool is_disable_voltage() { std::scoped_lock lock(w_mutex); bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; bool ev_unset = ((control_word >> CW_Enable_Voltage) & 1U) == 0U; if (fr_unset && ev_unset) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Disable Voltage."); return true; } return false; } bool is_switch_on() { std::scoped_lock lock(w_mutex); bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; bool eo_unset = ((control_word >> CW_Enable_Operation) & 1U) == 0U; bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; if (fr_unset && eo_unset && qs_set && ev_set && so_set) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Switch On."); return true; } return false; } bool is_enable_operation() { std::scoped_lock lock(w_mutex); bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; bool eo_set = ((control_word >> CW_Enable_Operation) & 1U) == 1U; bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; if (fr_unset && eo_set && qs_set && ev_set && so_set) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Enable Operation."); return true; } return false; } bool is_quickstop() { std::scoped_lock lock(w_mutex); bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; bool qs_unset = ((control_word >> CW_Quick_Stop) & 1U) == 0U; bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; if (fr_unset && qs_unset && ev_set) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Quick Stop."); return true; } return false; } bool is_faul_reset() { std::scoped_lock lock(w_mutex); bool fr_set = ((control_word >> CW_Fault_Reset) & 1U) == 1U; if (fr_set) { RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Fault Reset."); return true; } return false; } // This function gets called every time a value is written to the local object // dictionary by an SDO or RPDO. void OnWrite(uint16_t idx, uint8_t subidx) noexcept override { // System State if (idx == CiaRegister::ControlWord && subidx == 0) { { std::scoped_lock lock(w_mutex); control_word = (*this)[CiaRegister::ControlWord][0]; } set_new_status_word_and_state(); { std::scoped_lock lock(w_mutex); (*this)[CiaRegister::StatusWord][0] = status_word; this->TpdoEvent(1); } } // Operation Mode if (idx == CiaRegister::OperationMode && subidx == 0) { int8_t mode = (*this)[CiaRegister::OperationMode][0]; switch (mode) { case No_Mode: case Profiled_Position: case Velocity: case Profiled_Velocity: case Profiled_Torque: case Reserved: case Homing: case Interpolated_Position: case Cyclic_Synchronous_Position: case Cyclic_Synchronous_Velocity: case Cyclic_Synchronous_Torque: operation_mode.store(mode); break; default: std::cout << "Error: Master tried to set unknown operation mode." << std::endl; } // RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched to mode %hhi.", mode); (*this)[CiaRegister::ModeOfOperationDisplay][0] = (int8_t)(mode); this->TpdoEvent(1); } } }; class DelayedBootSlave : public ros2_canopen::CIA402MockSlave { public: using ros2_canopen::CIA402MockSlave::CIA402MockSlave; void DelayedReset() { RCLCPP_WARN(rclcpp::get_logger("cia402_slave"), "Delaying CANopen Boot-Up Frame..."); std::thread( [this]() { std::this_thread::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Now sending Boot-Up frame..."); this->canopen::BasicSlave::Reset(); }) .detach(); } }; class CIA402Slave : public BaseSlave { public: explicit CIA402Slave(const std::string & node_name, bool intra_process_comms = false) : BaseSlave(node_name, intra_process_comms) { } void run() override { io::IoGuard io_guard; io::Context ctx; io::Poll poll(ctx); ev::Loop loop(poll.get_poll()); auto exec = loop.get_executor(); io::Timer timer(poll, exec, CLOCK_MONOTONIC); io::CanController ctrl(can_interface_name_.c_str()); io::CanChannel chan(poll, exec); chan.open(ctrl); auto sigset_ = lely::io::SignalSet(poll, exec); // Watch for Ctrl+C or process termination. sigset_.insert(SIGHUP); sigset_.insert(SIGINT); sigset_.insert(SIGTERM); sigset_.submit_wait( [&](int /*signo*/) { // If the signal is raised again, terminate immediately. sigset_.clear(); // Perform a clean shutdown. ctx.shutdown(); }); DelayedBootSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); slave.DelayedReset(); RCLCPP_INFO(this->get_logger(), "Created cia402 slave for node_id %i.", node_id_); loop.run(); ctx.shutdown(); RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); rclcpp::shutdown(); } }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp ================================================ // Copyright 2023 ROS-Industrial // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef __MOTIONGENERATOR_H__ #define __MOTIONGENERATOR_H__ // Copyright (c) 2016 AerDronix, https://aerdronix.wordpress.com/ // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal // in the Software without restriction, including without limitation the rights // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: // // The above copyright notice and this permission notice shall be included in all // copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. #include /** * Generates the analytical solution for the trapezoidal motion. * *

* Usage: * // Includes * #include "MotionGenerator.h" * * Initialization * * @param int aVelMax maximum velocity (units/s) * @param int aAccMax maximum acceleration (units/s^2) * @param int aInitPos initial position (units) * // Define the MotionGenerator object MotionGenerator *trapezoidalProfile = new MotionGenerator(100, 400, 0); // Retrieve calculated position double positionRef = 1000; double position = trapezoidalProfile->update(positionRef) // Retrieve current velocity double velocity = trapezoidalProfile->getVelocity(); // Retrieve current acceleration double acceleration = trapezoidalProfile->getAcceleration(); // Check if profile is finished if (trapezoidalProfile->getFinished()) {}; // Reset internal state trapezoidalProfile->reset(); * * @author AerDronix * @web https://aerdronix.wordpress.com/ * @version 1.0 * @since 2016-12-22 */ class MotionGenerator { public: /** * Constructor * * @param int aVelocityMax maximum velocity * @param int aAccelerationMax maximum acceleration */ MotionGenerator(double aMaxVel, double aMaxAcc, double aInitPos); void init(); /** * Updates the state, generating new setpoints * * @param aSetpoint The current setpoint. */ double update(double aPosRef); double getVelocity(); double getAcceleration(); bool getFinished(); void setMaxVelocity(double aMaxVel); void setMaxAcceleration(double aMaxAcc); void setInitPosition(double aInitPos); void reset(); private: /** * Increments the state number. * * @see currentState */ void calculateTrapezoidalProfile(double); short int sign(double aVal); double maxVel; double maxAcc; double initPos; double pos; double vel; double acc; double oldPos; double oldPosRef; double oldVel; double dBrk; double dAcc; double dVel; double dDec; double dTot; double tBrk; double tAcc; double tVel; double tDec; double velSt; std::chrono::time_point oldTime; std::chrono::time_point lastTime; std::chrono::duration deltaTime; short int signM; // 1 = positive change, -1 = negative change bool shape; // true = trapezoidal, false = triangular bool isFinished; }; #endif ================================================ FILE: canopen_fake_slaves/launch/basic_slave.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os import sys sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa import launch import launch.actions import launch.events from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution from launch.actions import DeclareLaunchArgument import launch_ros import launch_ros.events import launch_ros.events.lifecycle import lifecycle_msgs.msg def generate_launch_description(): path_to_test = os.path.dirname(__file__) node_id_arg = DeclareLaunchArgument( "node_id", default_value=TextSubstitution(text="2"), description="CANopen node id the mock slave shall have.", ) slave_config_arg = DeclareLaunchArgument( "slave_config", default_value=TextSubstitution( text=os.path.join(path_to_test, "..", "config", "simple_slave.eds") ), description="Path to eds file to be used for the slave.", ) can_interface_name_arg = DeclareLaunchArgument( "can_interface_name", default_value=TextSubstitution(text="vcan0"), description="CAN interface to be used by mock slave.", ) node_name_arg = DeclareLaunchArgument( "node_name", default_value=TextSubstitution(text="basic_slave_node"), description="Name of the node.", ) slave_node = launch_ros.actions.LifecycleNode( name=LaunchConfiguration("node_name"), namespace="", package="canopen_fake_slaves", output="screen", executable="basic_slave_node", parameters=[ { "slave_config": LaunchConfiguration("slave_config"), "node_id": LaunchConfiguration("node_id"), "can_interface_name": LaunchConfiguration("can_interface_name"), } ], ) slave_inactive_state_handler = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=slave_node, goal_state="inactive", handle_once=True, entities=[ launch.actions.LogInfo( msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'." ), launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(slave_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, ) ), ], ), ) slave_configure = launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(slave_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, ) ) ld = launch.LaunchDescription() ld.add_action(node_id_arg) ld.add_action(slave_config_arg) ld.add_action(can_interface_name_arg) ld.add_action(node_name_arg) ld.add_action(slave_inactive_state_handler) ld.add_action(slave_node) ld.add_action(slave_configure) return ld ================================================ FILE: canopen_fake_slaves/launch/cia402_slave.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os import sys sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa import launch import launch.actions import launch.events from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution from launch.actions import DeclareLaunchArgument import launch_ros import launch_ros.events import launch_ros.events.lifecycle import lifecycle_msgs.msg def generate_launch_description(): path_to_test = os.path.dirname(__file__) node_id_arg = DeclareLaunchArgument( "node_id", default_value=TextSubstitution(text="2"), description="CANopen node id the mock slave shall have.", ) slave_config_arg = DeclareLaunchArgument( "slave_config", default_value=TextSubstitution( text=os.path.join(path_to_test, "..", "config", "cia402_slave.eds") ), description="Path to eds file to be used for the slave.", ) can_interface_name_arg = DeclareLaunchArgument( "can_interface_name", default_value=TextSubstitution(text="vcan0"), description="CAN interface to be used by mock slave.", ) node_name_arg = DeclareLaunchArgument( "node_name", default_value=TextSubstitution(text="basic_slave_node"), description="Name of the node.", ) slave_node = launch_ros.actions.LifecycleNode( name=LaunchConfiguration("node_name"), namespace="", package="canopen_fake_slaves", output="screen", executable="cia402_slave_node", parameters=[ { "slave_config": LaunchConfiguration("slave_config"), "node_id": LaunchConfiguration("node_id"), "can_interface_name": LaunchConfiguration("can_interface_name"), } ], ) slave_inactive_state_handler = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=slave_node, goal_state="inactive", handle_once=True, entities=[ launch.actions.LogInfo( msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'." ), launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(slave_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, ) ), ], ), ) slave_configure = launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(slave_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, ) ) ld = launch.LaunchDescription() ld.add_action(node_id_arg) ld.add_action(slave_config_arg) ld.add_action(can_interface_name_arg) ld.add_action(node_name_arg) ld.add_action(slave_inactive_state_handler) ld.add_action(slave_node) ld.add_action(slave_configure) return ld ================================================ FILE: canopen_fake_slaves/package.xml ================================================ canopen_fake_slaves 0.3.2 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 ament_cmake lely_core_libraries lifecycle_msgs rclcpp rclcpp_lifecycle ament_lint_auto ament_cmake ================================================ FILE: canopen_fake_slaves/src/basic_slave.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_fake_slaves/basic_slave.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char * argv[]) { rclcpp::InitOptions options; options.shutdown_on_signal = true; rclcpp::init(argc, argv, options, rclcpp::SignalHandlerOptions::All); rclcpp::executors::SingleThreadedExecutor executor; auto canopen_slave = std::make_shared("basic_slave"); executor.add_node(canopen_slave->get_node_base_interface()); executor.spin(); return 0; } ================================================ FILE: canopen_fake_slaves/src/cia402_slave.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_fake_slaves/cia402_slave.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; auto canopen_slave = std::make_shared("cia402_slave"); executor.add_node(canopen_slave->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); return 0; } ================================================ FILE: canopen_fake_slaves/src/motion_generator.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_fake_slaves/motion_generator.hpp" #include #include MotionGenerator::MotionGenerator(double aMaxVel, double aMaxAcc, double aInitPos) : maxVel(aMaxVel), maxAcc(aMaxAcc), initPos(aInitPos), oldPosRef(aInitPos) { init(); } void MotionGenerator::init() { // Time variables oldTime = std::chrono::high_resolution_clock::now(); lastTime = oldTime; deltaTime = std::chrono::milliseconds(0); // State variables reset(); // Misc signM = 1; // 1 = positive change, -1 = negative change shape = true; // true = trapezoidal, false = triangular isFinished = false; } double MotionGenerator::update(double posRef) { if (oldPosRef != posRef) // reference changed { isFinished = false; // Shift state variables oldPosRef = posRef; oldPos = pos; oldVel = vel; oldTime = lastTime; // Calculate braking time and distance (in case is needed) tBrk = abs(oldVel) / maxAcc; dBrk = tBrk * abs(oldVel) / 2; // Calculate Sign of motion signM = sign(posRef - (oldPos + sign(oldVel) * dBrk)); if (signM != sign(oldVel)) // means brake is needed { tAcc = (maxVel / maxAcc); dAcc = tAcc * (maxVel / 2); } else { tBrk = 0; dBrk = 0; tAcc = (maxVel - abs(oldVel)) / maxAcc; dAcc = tAcc * (maxVel + abs(oldVel)) / 2; } // Calculate total distance to go after braking dTot = abs(posRef - oldPos + signM * dBrk); tDec = maxVel / maxAcc; dDec = tDec * (maxVel) / 2; dVel = dTot - (dAcc + dDec); tVel = dVel / maxVel; if (tVel > 0) // trapezoidal shape shape = true; else // triangular shape { shape = false; // Recalculate distances and periods if (signM != sign(oldVel)) // means brake is needed { velSt = sqrt(maxAcc * (dTot)); tAcc = (velSt / maxAcc); dAcc = tAcc * (velSt / 2); } else { tBrk = 0; dBrk = 0; dTot = abs(posRef - oldPos); // recalculate total distance velSt = sqrt(0.5 * oldVel * oldVel + maxAcc * dTot); tAcc = (velSt - abs(oldVel)) / maxAcc; dAcc = tAcc * (velSt + abs(oldVel)) / 2; } tDec = velSt / maxAcc; dDec = tDec * (velSt) / 2; } } auto time = std::chrono::high_resolution_clock::now(); // current time // Calculate time since last set-point change deltaTime = (time - oldTime); // Calculate new setpoint calculateTrapezoidalProfile(posRef); // Update last time lastTime = time; // calculateTrapezoidalProfile(setpoint); return pos; } void MotionGenerator::calculateTrapezoidalProfile(double posRef) { double t = deltaTime.count(); // conversion from milliseconds to seconds if (shape) // trapezoidal shape { if (t <= (tBrk + tAcc)) { pos = oldPos + oldVel * t + signM * 0.5 * maxAcc * t * t; vel = oldVel + signM * maxAcc * t; acc = signM * maxAcc; } else if (t > (tBrk + tAcc) && t < (tBrk + tAcc + tVel)) { pos = oldPos + signM * (-dBrk + dAcc + maxVel * (t - tBrk - tAcc)); vel = signM * maxVel; acc = 0; } else if (t >= (tBrk + tAcc + tVel) && t < (tBrk + tAcc + tVel + tDec)) { pos = oldPos + signM * (-dBrk + dAcc + dVel + maxVel * (t - tBrk - tAcc - tVel) - 0.5 * maxAcc * (t - tBrk - tAcc - tVel) * (t - tBrk - tAcc - tVel)); vel = signM * (maxVel - maxAcc * (t - tBrk - tAcc - tVel)); acc = -signM * maxAcc; } else { pos = posRef; vel = 0; acc = 0; isFinished = true; } } else // triangular shape { if (t <= (tBrk + tAcc)) { pos = oldPos + oldVel * t + signM * 0.5 * maxAcc * t * t; vel = oldVel + signM * maxAcc * t; acc = signM * maxAcc; } else if (t > (tBrk + tAcc) && t < (tBrk + tAcc + tDec)) { pos = oldPos + signM * (-dBrk + dAcc + velSt * (t - tBrk - tAcc) - 0.5 * maxAcc * (t - tBrk - tAcc) * (t - tBrk - tAcc)); vel = signM * (velSt - maxAcc * (t - tBrk - tAcc)); acc = -signM * maxAcc; } else { pos = posRef; vel = 0; acc = 0; isFinished = true; } } } // Getters and setters bool MotionGenerator::getFinished() { return isFinished; } double MotionGenerator::getVelocity() { return vel; } double MotionGenerator::getAcceleration() { return acc; } void MotionGenerator::setMaxVelocity(double aMaxVel) { maxVel = aMaxVel; } void MotionGenerator::setMaxAcceleration(double aMaxAcc) { maxAcc = aMaxAcc; } void MotionGenerator::setInitPosition(double aInitPos) { initPos = aInitPos; pos = aInitPos; oldPos = aInitPos; oldPosRef = aInitPos; } short int MotionGenerator::sign(double aVal) { if (aVal < 0) return -1; else if (aVal > 0) return 1; else { return 0; } } void MotionGenerator::reset() { // Reset all state variables pos = initPos; oldPos = initPos; oldPosRef = initPos; vel = 0; acc = 0; oldVel = 0; dBrk = 0; dAcc = 0; dVel = 0; dDec = 0; dTot = 0; tBrk = 0; tAcc = 0; tVel = 0; tDec = 0; velSt = 0; } ================================================ FILE: canopen_interfaces/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ 0.2.9 (2024-04-16) ------------------ 0.3.1 (2025-06-23) ------------------ 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Contributors: ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro ================================================ FILE: canopen_interfaces/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_interfaces) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/COHeartbeatID.srv" "srv/CONmtID.srv" "srv/CORead.srv" "srv/COReadID.srv" "srv/COWrite.srv" "srv/COWriteID.srv" "srv/COTargetDouble.srv" "srv/CONode.srv" "msg/COData.msg" ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # uncomment the line when a copyright and license is not present in all source files #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() ament_package() ================================================ FILE: canopen_interfaces/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_interfaces/msg/COData.msg ================================================ uint16 index uint8 subindex uint32 data ================================================ FILE: canopen_interfaces/package.xml ================================================ canopen_interfaces 0.3.2 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 ament_cmake ament_lint_auto ament_lint_common rosidl_default_generators rosidl_default_runtime rosidl_interface_packages ament_cmake ================================================ FILE: canopen_interfaces/srv/COHeartbeatID.srv ================================================ uint8 nodeid uint16 heartbeat #ms --- bool success ================================================ FILE: canopen_interfaces/srv/CONmtID.srv ================================================ uint8 nmtcommand uint8 nodeid --- bool success ================================================ FILE: canopen_interfaces/srv/CONode.srv ================================================ uint8 nodeid --- bool success ================================================ FILE: canopen_interfaces/srv/CORead.srv ================================================ uint16 index uint8 subindex --- bool success uint32 data ================================================ FILE: canopen_interfaces/srv/COReadID.srv ================================================ uint8 CANOPEN_DATATYPE_INT8 = 0x02 uint8 CANOPEN_DATATYPE_INT16 = 0x03 uint8 CANOPEN_DATATYPE_INT32 = 0x04 uint8 CANOPEN_DATATYPE_UINT8 = 0x05 uint8 CANOPEN_DATATYPE_UINT16 = 0x06 uint8 CANOPEN_DATATYPE_UINT32 = 0x07 uint8 nodeid uint16 index uint8 subindex # 8 = uint8_t, 16 = uint16_t, 32 = uint32_t uint8 canopen_datatype --- bool success uint32 data ================================================ FILE: canopen_interfaces/srv/COTargetDouble.srv ================================================ float64 target --- bool success ================================================ FILE: canopen_interfaces/srv/COWrite.srv ================================================ uint16 index uint8 subindex uint32 data --- bool success ================================================ FILE: canopen_interfaces/srv/COWriteID.srv ================================================ uint8 CANOPEN_DATATYPE_INT8 = 0x02 uint8 CANOPEN_DATATYPE_INT16 = 0x03 uint8 CANOPEN_DATATYPE_INT32 = 0x04 uint8 CANOPEN_DATATYPE_UINT8 = 0x05 uint8 CANOPEN_DATATYPE_UINT16 = 0x06 uint8 CANOPEN_DATATYPE_UINT32 = 0x07 int8 nodeid uint16 index uint8 subindex uint32 data # 8 = uint8_t, 16 = uint16_t, 32 = uint32_t uint8 canopen_datatype --- bool success ================================================ FILE: canopen_master_driver/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.2.9 (2024-04-16) ------------------ * Add timeouts * Contributors: Vishnuprasad Prachandabhanu 0.3.2 (2025-12-05) ------------------ 0.3.1 (2025-06-23) ------------------ 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Merge pull request `#220 `_ from ipa-vsp/feature/timeout-config Add timeouts * Revert timeout change to master since I'm not providing a way to set that timeout. * Make 20ms a default argument of the master & driver bridges. * timeout for booting slave * Contributors: Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix master lifecycle * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos ================================================ FILE: canopen_master_driver/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_master_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(lely_core_libraries REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) add_library(lely_master_bridge src/lely_master_bridge.cpp ) target_compile_features(lely_master_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lely_master_bridge PUBLIC -Wl,--no-undefined) target_include_directories(lely_master_bridge PUBLIC $ $ ${lely_core_libraries_INCLUDE_DIRS} ) target_link_libraries(lely_master_bridge PUBLIC ${lely_core_libraries_LIBRARIES} canopen_core::node_canopen_master ) add_library(node_canopen_basic_master src/node_interfaces/node_canopen_basic_master.cpp ) target_compile_features(node_canopen_basic_master PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_basic_master PUBLIC -Wl,--no-undefined) target_include_directories(node_canopen_basic_master PUBLIC $ $) target_link_libraries(node_canopen_basic_master PUBLIC ${canopen_interfaces_TARGETS} canopen_core::node_canopen_master lely_master_bridge ) add_library(lifecycle_master_driver src/lifecycle_master_driver.cpp ) target_compile_features(lifecycle_master_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_master_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_master_driver PUBLIC $ $) target_link_libraries(lifecycle_master_driver PUBLIC canopen_core::node_canopen_master node_canopen_basic_master rclcpp_components::component rclcpp_lifecycle::rclcpp_lifecycle ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(lifecycle_master_driver PRIVATE "CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(lifecycle_master_driver "ros2_canopen::LifecycleMasterDriver") set(node_plugins "${node_plugins}ros2_canopen::LifecycleMasterDriver;$\n") add_library(master_driver src/master_driver.cpp ) target_compile_features(master_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(master_driver PUBLIC -Wl,--no-undefined) target_include_directories(master_driver PUBLIC $ $) target_link_libraries(master_driver PUBLIC canopen_core::node_canopen_master node_canopen_basic_master rclcpp_components::component ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(master_driver PRIVATE "CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(master_driver "ros2_canopen::MasterDriver") set(node_plugins "${node_plugins}ros2_canopen::MasterDriver;$\n") install( DIRECTORY include/ DESTINATION include ) install( TARGETS lifecycle_master_driver master_driver node_canopen_basic_master lely_master_bridge EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( master_driver lifecycle_master_driver node_canopen_basic_master lely_master_bridge ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( canopen_core canopen_interfaces lely_core_libraries rclcpp rclcpp_components rclcpp_lifecycle ) ament_package() ================================================ FILE: canopen_master_driver/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_master_driver/include/canopen_master_driver/lely_master_bridge.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef LELY_MASTER_BRIDGE_HPP #define LELY_MASTER_BRIDGE_HPP #include #include #include "lely/coapp/master.hpp" #include #include #include #include #include #include "canopen_core/exchange.hpp" using namespace std::literals::chrono_literals; namespace ros2_canopen { /** * @brief Lely Master Bridge * * Creates services to pass values between the CANopen executor and ROS executor. * This is important because certain functions can only be called from within * the thread of the CANopen executor. * */ class LelyMasterBridge : public lely::canopen::AsyncMaster { std::shared_ptr> sdo_read_data_promise; ///< Pointer to Promise for read service calls. std::shared_ptr> sdo_write_data_promise; ///< Pointer to Promise for write service calls std::promise nmt_promise; ///< Pointer to Promise for nmt service calls std::mutex sdo_mutex; ///< Mutex to guard sdo objects bool running; ///< Bool to indicate whether an sdo call is running std::condition_variable sdo_cond; ///< Condition variable to sync service calls (one at a time) uint8_t node_id; ///< Node id of the master public: /** * @brief Construct a new Lely Master Bridge object * * @param [in] exec Pointer to the executor * @param [in] timer Pointer to the timer * @param [in] chan Pointer to the channel * @param [in] dcf_txt Path to the DCF file * @param [in] dcf_bin Path to the DCF bin file * @param [in] id CANopen node id of the master */ LelyMasterBridge( ev_exec_t * exec, lely::io::TimerBase & timer, lely::io::CanChannelBase & chan, const std::string & dcf_txt, const std::string & dcf_bin = "", uint8_t id = (uint8_t)255U) : lely::canopen::AsyncMaster(exec, timer, chan, dcf_txt, dcf_bin, id), node_id(id) { } /** * @brief Asynchronous call for writing to an SDO * * @param [in] id CANopen node id of the target device * @param [in] data Data to write * @param [in] datatype Datatype of the data to write * @return std::future A future that indicates if the * write Operation was successful. */ std::future async_write_sdo(uint8_t id, COData data, uint8_t datatype); /** * @brief * * @param [in] id CANopen node id of the target device * @param [in] data Data to read, value is disregarded * @param [in] datatype Datatype of the data to read * @return std::future A future that returns the read result * once the process finished. */ std::future async_read_sdo(uint8_t id, COData data, uint8_t datatype); /** * @brief async_write_nmt * * @param id * @param command * @return std::future * */ std::future async_write_nmt(uint8_t id, uint8_t command); template void submit_write_sdo(uint8_t id, uint16_t idx, uint8_t subidx, T value) { this->SubmitWrite( this->GetExecutor(), id, idx, subidx, value, [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable { if (ec) { this->sdo_write_data_promise->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); } else { this->sdo_write_data_promise->set_value(true); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, 20ms); } template void submit_read_sdo(uint8_t id, uint16_t idx, uint8_t subidx) { this->SubmitRead( this->GetExecutor(), id, idx, subidx, [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable { if (ec) { this->sdo_read_data_promise->set_exception( lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); } else { COData codata = {idx, subidx, 0U}; std::memcpy(&codata.data_, &value, sizeof(T)); this->sdo_read_data_promise->set_value(codata); } std::unique_lock lck(this->sdo_mutex); this->running = false; this->sdo_cond.notify_one(); }, 20ms); } }; } // namespace ros2_canopen #endif // LELY_MASTER_BRIDGE_HPP ================================================ FILE: canopen_master_driver/include/canopen_master_driver/lifecycle_master_driver.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef LIFECYCLE_MASTER_DRIVER_HPP #define LIFECYCLE_MASTER_DRIVER_HPP #include #include #include #include "canopen_core/master_node.hpp" #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" namespace ros2_canopen { /** * @brief Lifecycle Master Node * * This class implements the Lifecycle master interface. * It uses the Lely Master Bridge and exposes a ROS node * interface. * */ class LifecycleMasterDriver : public ros2_canopen::LifecycleCanopenMaster { std::shared_ptr> node_canopen_basic_master_; public: explicit LifecycleMasterDriver(const rclcpp::NodeOptions & node_options); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_master_driver/include/canopen_master_driver/master_driver.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef MASTER_DRIVER_HPP #define MASTER_DRIVER_HPP #include "canopen_core/master_node.hpp" #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" namespace ros2_canopen { /** * @brief Master Node * * This class implements the Lifecycle master interface. * It uses the Lely Master Bridge and exposes a ROS node * interface. * */ class MasterDriver : public ros2_canopen::CanopenMaster { std::shared_ptr> node_canopen_basic_master_; public: explicit MasterDriver(const rclcpp::NodeOptions & node_options); }; } // namespace ros2_canopen #endif ================================================ FILE: canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_BASIC_MASTER_HPP_ #define NODE_CANOPEN_BASIC_MASTER_HPP_ #include "canopen_core/node_interfaces/node_canopen_master.hpp" #include "canopen_interfaces/srv/co_read_id.hpp" #include "canopen_interfaces/srv/co_write_id.hpp" #include "canopen_master_driver/lely_master_bridge.hpp" namespace ros2_canopen { namespace node_interfaces { template class NodeCanopenBasicMaster : public ros2_canopen::node_interfaces::NodeCanopenMaster { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: std::shared_ptr master_bridge_; rclcpp::Service::SharedPtr sdo_read_service; rclcpp::Service::SharedPtr sdo_write_service; public: NodeCanopenBasicMaster(NODETYPE * node) : ros2_canopen::node_interfaces::NodeCanopenMaster(node) { this->activated_.load(); RCLCPP_INFO(this->node_->get_logger(), "NodeCanopenBasicMaster"); } virtual ~NodeCanopenBasicMaster() {} virtual void activate(bool called_from_base) override; virtual void deactivate(bool called_from_base) override; virtual void init(bool called_from_base) override; /** * @brief Read Service Data Object * * This Service is only available when the node is in active lifecycle state. * It will return with success false in any other lifecycle state and log an * RCLCPP_ERROR. * * @param request * @param response */ void on_sdo_read( const std::shared_ptr request, std::shared_ptr response); /** * @brief Write Service Data Object * * This service is only available when the node is in active lifecycle state. * It will return with success false in any other lifecycle state and log an * RCLCPP_ERROR. * * @param request * @param response */ void on_sdo_write( const std::shared_ptr request, std::shared_ptr response); }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ #define NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ #include "canopen_master_driver/lely_master_bridge.hpp" #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" using namespace ros2_canopen::node_interfaces; template void NodeCanopenBasicMaster::activate(bool called_from_base) { master_bridge_ = std::make_shared( *(this->exec_), *(this->timer_), *(this->chan_), this->master_dcf_, this->master_bin_, this->node_id_); master_bridge_->SetTimeout(std::chrono::milliseconds(this->timeout_)); this->master_ = std::static_pointer_cast(master_bridge_); } template void NodeCanopenBasicMaster::deactivate(bool called_from_base) { master_bridge_.reset(); this->master_.reset(); } template void NodeCanopenBasicMaster::on_sdo_read( const std::shared_ptr request, std::shared_ptr response) { if (this->activated_.load()) { ros2_canopen::COData data = {request->index, request->subindex, 0U}; std::future f = this->master_bridge_->async_read_sdo(request->nodeid, data, request->canopen_datatype); f.wait(); try { response->data = f.get().data_; response->success = true; } catch (std::exception & e) { RCLCPP_ERROR(this->node_->get_logger(), e.what()); response->success = false; } } else { RCLCPP_ERROR( this->node_->get_logger(), "LifecycleMasterNode is not in active state. SDO read service is not available."); response->success = false; } } template void NodeCanopenBasicMaster::on_sdo_write( const std::shared_ptr request, std::shared_ptr response) { if (this->activated_.load()) { ros2_canopen::COData data = {request->index, request->subindex, request->data}; std::future f = this->master_bridge_->async_write_sdo(request->nodeid, data, request->canopen_datatype); f.wait(); try { response->success = f.get(); } catch (std::exception & e) { RCLCPP_ERROR(this->node_->get_logger(), e.what()); response->success = false; } } else { RCLCPP_ERROR( this->node_->get_logger(), "LifecycleMasterNode is not in active state. SDO write service is not available."); response->success = false; } } template <> void NodeCanopenBasicMaster::init(bool called_from_base) { // declare services sdo_read_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_read").c_str(), std::bind( &ros2_canopen::node_interfaces::NodeCanopenBasicMaster::on_sdo_read, this, std::placeholders::_1, std::placeholders::_2)); sdo_write_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_write").c_str(), std::bind( &ros2_canopen::node_interfaces::NodeCanopenBasicMaster::on_sdo_write, this, std::placeholders::_1, std::placeholders::_2)); } template <> void NodeCanopenBasicMaster::init(bool called_from_base) { // declare services sdo_read_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_read").c_str(), std::bind( &ros2_canopen::node_interfaces::NodeCanopenBasicMaster< rclcpp_lifecycle::LifecycleNode>::on_sdo_read, this, std::placeholders::_1, std::placeholders::_2)); sdo_write_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_write").c_str(), std::bind( &ros2_canopen::node_interfaces::NodeCanopenBasicMaster< rclcpp_lifecycle::LifecycleNode>::on_sdo_write, this, std::placeholders::_1, std::placeholders::_2)); } #endif ================================================ FILE: canopen_master_driver/include/canopen_master_driver/visibility_control.h ================================================ // Copyright 2023 ROS-Industrial // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ #define CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_MASTER_DRIVER_EXPORT __attribute__((dllexport)) #define CANOPEN_MASTER_DRIVER_IMPORT __attribute__((dllimport)) #else #define CANOPEN_MASTER_DRIVER_EXPORT __declspec(dllexport) #define CANOPEN_MASTER_DRIVER_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY #define CANOPEN_MASTER_DRIVER_PUBLIC CANOPEN_MASTER_DRIVER_EXPORT #else #define CANOPEN_MASTER_DRIVER_PUBLIC CANOPEN_MASTER_DRIVER_IMPORT #endif #define CANOPEN_MASTER_DRIVER_PUBLIC_TYPE CANOPEN_MASTER_DRIVER_PUBLIC #define CANOPEN_MASTER_DRIVER_LOCAL #else #define CANOPEN_MASTER_DRIVER_EXPORT __attribute__((visibility("default"))) #define CANOPEN_MASTER_DRIVER_IMPORT #if __GNUC__ >= 4 #define CANOPEN_MASTER_DRIVER_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_MASTER_DRIVER_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_MASTER_DRIVER_PUBLIC #define CANOPEN_MASTER_DRIVER_LOCAL #endif #define CANOPEN_MASTER_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_master_driver/package.xml ================================================ canopen_master_driver 0.3.2 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 ament_cmake_ros canopen_core canopen_interfaces lely_core_libraries rclcpp rclcpp_components rclcpp_lifecycle ament_lint_auto ament_lint_common ament_cmake ================================================ FILE: canopen_master_driver/src/lely_master_bridge.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_master_driver/lely_master_bridge.hpp" #include #include using namespace std::literals::chrono_literals; namespace ros2_canopen /* constant-expression */ { std::future LelyMasterBridge::async_write_sdo(uint8_t id, COData data, uint8_t datatype) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; sdo_write_data_promise = std::make_shared>(); try { switch (datatype) { case CO_DEFTYPE_INTEGER8: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; case CO_DEFTYPE_INTEGER16: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; case CO_DEFTYPE_INTEGER32: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; case CO_DEFTYPE_UNSIGNED8: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; case CO_DEFTYPE_UNSIGNED16: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; case CO_DEFTYPE_UNSIGNED32: submit_write_sdo(id, data.index_, data.subindex_, data.data_); break; default: throw lely::canopen::SdoError( id, data.index_, data.subindex_, lely::canopen::SdoErrc::ERROR, "Unknown datatype"); break; } } catch (...) { this->sdo_write_data_promise->set_exception(std::current_exception()); this->running = false; } return sdo_write_data_promise->get_future(); } std::future LelyMasterBridge::async_read_sdo(uint8_t id, COData data, uint8_t datatype) { std::unique_lock lck(sdo_mutex); if (running) { sdo_cond.wait(lck); } running = true; sdo_read_data_promise = std::make_shared>(); try { switch (datatype) { case CO_DEFTYPE_INTEGER8: submit_read_sdo(id, data.index_, data.subindex_); break; case CO_DEFTYPE_INTEGER16: submit_read_sdo(id, data.index_, data.subindex_); break; case CO_DEFTYPE_INTEGER32: submit_read_sdo(id, data.index_, data.subindex_); break; case CO_DEFTYPE_UNSIGNED8: submit_read_sdo(id, data.index_, data.subindex_); break; case CO_DEFTYPE_UNSIGNED16: submit_read_sdo(id, data.index_, data.subindex_); break; case CO_DEFTYPE_UNSIGNED32: submit_read_sdo(id, data.index_, data.subindex_); break; default: throw lely::canopen::SdoError( id, data.index_, data.subindex_, lely::canopen::SdoErrc::ERROR, "Unknown datatype"); break; } } catch (...) { this->sdo_read_data_promise->set_exception(std::current_exception()); this->running = false; } return sdo_read_data_promise->get_future(); } std::future LelyMasterBridge::async_write_nmt(uint8_t id, uint8_t command) { lely::canopen::NmtCommand command_ = static_cast(command); switch (command_) { case lely::canopen::NmtCommand::ENTER_PREOP: case lely::canopen::NmtCommand::RESET_COMM: case lely::canopen::NmtCommand::RESET_NODE: case lely::canopen::NmtCommand::START: case lely::canopen::NmtCommand::STOP: this->Command(command_, id); break; default: break; } nmt_promise.set_value(true); return nmt_promise.get_future(); } } // namespace ros2_canopen ================================================ FILE: canopen_master_driver/src/lifecycle_master_driver.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_master_driver/lifecycle_master_driver.hpp" namespace ros2_canopen { ros2_canopen::LifecycleMasterDriver::LifecycleMasterDriver(const rclcpp::NodeOptions & node_options) : LifecycleCanopenMaster(node_options) { node_canopen_master_ = std::make_shared>( this); } } // namespace ros2_canopen #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleMasterDriver) ================================================ FILE: canopen_master_driver/src/master_driver.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_master_driver/master_driver.hpp" namespace ros2_canopen { ros2_canopen::MasterDriver::MasterDriver(const rclcpp::NodeOptions & node_options) : CanopenMaster(node_options) { node_canopen_basic_master_ = std::make_shared>(this); node_canopen_master_ = std::static_pointer_cast( node_canopen_basic_master_); } } // namespace ros2_canopen #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::MasterDriver) ================================================ FILE: canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp ================================================ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp" using namespace ros2_canopen::node_interfaces; template class ros2_canopen::node_interfaces::NodeCanopenBasicMaster; template class ros2_canopen::node_interfaces::NodeCanopenBasicMaster< rclcpp_lifecycle::LifecycleNode>; ================================================ FILE: canopen_master_driver/test/CMakeLists.txt ================================================ ament_add_gtest(test_node_canopen_basic_driver test_node_canopen_basic_master.cpp ) target_include_directories(test_node_canopen_basic_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_canopen_basic_driver ${canopen_interfaces_TARGETS} canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master lely_master_bridge master_driver node_canopen_basic_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gtest(test_node_canopen_basic_driver_ros test_node_canopen_basic_master_ros.cpp ) target_include_directories(test_node_canopen_basic_driver_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_canopen_basic_driver_ros ${canopen_interfaces_TARGETS} canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master lely_master_bridge master_driver node_canopen_basic_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gtest(test_master_driver_component test_master_driver_component.cpp ) target_include_directories(test_master_driver_component PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_master_driver_component ${canopen_interfaces_TARGETS} canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master lely_master_bridge lifecycle_master_driver master_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) ================================================ FILE: canopen_master_driver/test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=2 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=32 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1A00 20=0x1A01 21=0x1F25 22=0x1F55 23=0x1F80 24=0x1F81 25=0x1F82 26=0x1F84 27=0x1F85 28=0x1F86 29=0x1F87 30=0x1F88 31=0x1F89 32=0x1F8A [ManufacturerObjects] SupportedObjects=12 1=0x2000 2=0x2001 3=0x2200 4=0x2201 5=0x5800 6=0x5801 7=0x5A00 8=0x5A01 9=0x5C00 10=0x5C01 11=0x5E00 12=0x5E01 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=2 2=0x00000082 3=0x00000083 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=2 2=0x00000005 3=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=2 2=0x00000360 3=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=proxy_device_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=proxy_device_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=proxy_device_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=proxy_device_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_master_driver/test/test_master_driver_component.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include #include #include #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "gtest/gtest.h" using namespace rclcpp_components; TEST(MasterDriverComponent, test_load_lifecycle_master_driver) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_master_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[0]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } TEST(MasterDriverComponent, test_load_master_driver) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_master_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[1]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } ================================================ FILE: canopen_master_driver/test/test_node_canopen_basic_master.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "gtest/gtest.h" class RclCppFixture { public: RclCppFixture() { rclcpp::init(0, nullptr); node = new rclcpp::Node("Node"); interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); } virtual ~RclCppFixture() { rclcpp::shutdown(); delete (interface); delete (node); } rclcpp::Node * node; ros2_canopen::node_interfaces::NodeCanopenBasicMaster * interface; }; RclCppFixture g_rclcppfixture; TEST(NodeCanopenBasicMaster, test_bad_sequence_configure) { auto iface = static_cast( g_rclcppfixture.interface); EXPECT_ANY_THROW(iface->configure()); } TEST(NodeCanopenBasicMaster, test_bad_sequence_activate) { auto iface = static_cast( g_rclcppfixture.interface); EXPECT_ANY_THROW(iface->activate()); } TEST(NodeCanopenBasicMaster, test_good_sequence) { auto iface = static_cast( g_rclcppfixture.interface); try { iface->init(); } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("test"), e.what()); } EXPECT_NO_THROW(iface->configure()); } ================================================ FILE: canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "gtest/gtest.h" TEST(NodeCanopenBasicMaster, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp::Node * node = new rclcpp::Node("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter master_dcf("master_dcf", "master.dcf"); rclcpp::Parameter master_bin("master_bin", ""); rclcpp::Parameter can_interface_name("can_interface_name", "vcan0"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config("config", ""); node->set_parameter(container_name); node->set_parameter(master_dcf); node->set_parameter(master_bin); node->set_parameter(can_interface_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter master_dcf("master_dcf", "master.dcf"); rclcpp::Parameter master_bin("master_bin", ""); rclcpp::Parameter can_interface_name("can_interface_name", "vcan0"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config("config", ""); node->set_parameter(container_name); node->set_parameter(master_dcf); node->set_parameter(master_bin); node->set_parameter(can_interface_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } ================================================ FILE: canopen_proxy_driver/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.2.9 (2024-04-16) ------------------ 0.3.2 (2025-12-05) ------------------ * `#379 `_: Fix data conversion in the Lely Bridge to enable more data types and proper handling of Emcy in ros2_control * Fixed types handling in canopen_ros2_control. * Optimize debug output. * Contributors: Dr. Denis Stogl, Vishnuprasad Prachandabhanu 0.3.1 (2025-06-23) ------------------ 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Contributors: ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Use consistenlty (uppercase) HEX output for NodeID and Index. * Contributors: Christoph Hellmann Santos, Denis Štogl 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_proxy_driver/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_proxy_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(canopen_base_driver REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) add_library(node_canopen_proxy_driver src/node_interfaces/node_canopen_proxy_driver.cpp ) target_compile_features(node_canopen_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(node_canopen_proxy_driver PUBLIC -Wl,--no-undefined) target_include_directories(node_canopen_proxy_driver PUBLIC $ $) target_link_libraries(node_canopen_proxy_driver PUBLIC canopen_base_driver::node_canopen_base_driver ) add_library(lifecycle_proxy_driver src/lifecycle_proxy_driver.cpp ) target_compile_features(lifecycle_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_proxy_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_proxy_driver PUBLIC $ $) target_link_libraries(lifecycle_proxy_driver PUBLIC canopen_core::node_canopen_driver node_canopen_proxy_driver rclcpp_components::component rclcpp_lifecycle::rclcpp_lifecycle ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(lifecycle_proxy_driver PRIVATE "CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(lifecycle_proxy_driver "ros2_canopen::LifecycleProxyDriver") set(node_plugins "${node_plugins}ros2_canopen::LifecycleProxyDriver;$\n") add_library(proxy_driver src/proxy_driver.cpp ) target_compile_features(proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(proxy_driver PUBLIC -Wl,--no-undefined) target_include_directories(proxy_driver PUBLIC $ $) target_link_libraries(proxy_driver PUBLIC canopen_core::node_canopen_driver node_canopen_proxy_driver rclcpp_components::component ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(proxy_driver PRIVATE "CANOPEN_proxy_DRIVER_BUILDING_LIBRARY") rclcpp_components_register_nodes(proxy_driver "ros2_canopen::ProxyDriver") set(node_plugins "${node_plugins}ros2_canopen::ProxyDriver;$\n") install( DIRECTORY include/ DESTINATION include ) install( TARGETS lifecycle_proxy_driver proxy_driver node_canopen_proxy_driver EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( lifecycle_proxy_driver proxy_driver node_canopen_proxy_driver ) ament_export_targets( export_${PROJECT_NAME} ) ament_export_dependencies( canopen_base_driver canopen_core canopen_interfaces rclcpp rclcpp_components rclcpp_lifecycle ) ament_package() ================================================ FILE: canopen_proxy_driver/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_proxy_driver/config/concurrency_test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=6 NrOfTxPDO=6 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=48 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1402 16=0x1403 17=0x1404 18=0x1405 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1604 24=0x1605 25=0x1800 26=0x1801 27=0x1802 28=0x1803 29=0x1804 30=0x1805 31=0x1A00 32=0x1A01 33=0x1A02 34=0x1A03 35=0x1A04 36=0x1A05 37=0x1F25 38=0x1F55 39=0x1F80 40=0x1F81 41=0x1F82 42=0x1F84 43=0x1F85 44=0x1F86 45=0x1F87 46=0x1F88 47=0x1F89 48=0x1F8A [ManufacturerObjects] SupportedObjects=36 1=0x2000 2=0x2001 3=0x2002 4=0x2003 5=0x2004 6=0x2005 7=0x2200 8=0x2201 9=0x2202 10=0x2203 11=0x2204 12=0x2205 13=0x5800 14=0x5801 15=0x5802 16=0x5803 17=0x5804 18=0x5805 19=0x5A00 20=0x5A01 21=0x5A02 22=0x5A03 23=0x5A04 24=0x5A05 25=0x5C00 26=0x5C01 27=0x5C02 28=0x5C03 29=0x5C04 30=0x5C05 31=0x5E00 32=0x5E01 33=0x5E02 34=0x5E03 35=0x5E04 36=0x5E05 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=6 2=0x00000082 3=0x00000083 4=0x00000084 5=0x00000085 6=0x00000086 7=0x00000087 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1402] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1402sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1402sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000184 [1402sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1402sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1402sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1402sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1403] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1403sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1403sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000185 [1403sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1403sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1403sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1403sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1404] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1404sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1404sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000186 [1404sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1404sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1404sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1404sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1405] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1405sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1405sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000187 [1405sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1405sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1405sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1405sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1602] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1602Value] NrOfEntries=1 1=0x20020120 [1603] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1603Value] NrOfEntries=1 1=0x20030120 [1604] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1604Value] NrOfEntries=1 1=0x20040120 [1605] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1605Value] NrOfEntries=1 1=0x20050120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1802] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1802sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1802sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000204 [1802sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1802sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1802sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1802sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1802sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1803] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1803sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1803sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000205 [1803sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1803sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1803sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1803sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1803sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1804] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1804sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1804sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000206 [1804sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1804sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1804sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1804sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1804sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1805] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1805sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1805sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000207 [1805sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1805sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1805sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1805sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1805sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1A02] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A02Value] NrOfEntries=1 1=0x22020120 [1A03] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A03Value] NrOfEntries=1 1=0x22030120 [1A04] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A04Value] NrOfEntries=1 1=0x22040120 [1A05] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A05Value] NrOfEntries=1 1=0x22050120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=6 2=0x00000005 3=0x00000005 4=0x00000005 5=0x00000005 6=0x00000005 7=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=6 2=0x00000360 3=0x00000360 4=0x00000360 5=0x00000360 6=0x00000360 7=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=motioncontroller_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=motioncontroller_3: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2002] SubNumber=2 ParameterName=Mapped application objects for RPDO 3 ObjectType=0x09 [2002sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2002sub1] ParameterName=motioncontroller_4: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2003] SubNumber=2 ParameterName=Mapped application objects for RPDO 4 ObjectType=0x09 [2003sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2003sub1] ParameterName=motioncontroller_5: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2004] SubNumber=2 ParameterName=Mapped application objects for RPDO 5 ObjectType=0x09 [2004sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2004sub1] ParameterName=motioncontroller_6: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2005] SubNumber=2 ParameterName=Mapped application objects for RPDO 6 ObjectType=0x09 [2005sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2005sub1] ParameterName=motioncontroller_7: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=motioncontroller_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=motioncontroller_3: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2202] SubNumber=2 ParameterName=Mapped application objects for TPDO 3 ObjectType=0x09 [2202sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2202sub1] ParameterName=motioncontroller_4: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2203] SubNumber=2 ParameterName=Mapped application objects for TPDO 4 ObjectType=0x09 [2203sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2203sub1] ParameterName=motioncontroller_5: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2204] SubNumber=2 ParameterName=Mapped application objects for TPDO 5 ObjectType=0x09 [2204sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2204sub1] ParameterName=motioncontroller_6: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2205] SubNumber=2 ParameterName=Mapped application objects for TPDO 6 ObjectType=0x09 [2205sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2205sub1] ParameterName=motioncontroller_7: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5802] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000104 [5803] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000105 [5804] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000106 [5805] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000107 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5A02] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A02Value] NrOfEntries=1 1=0x40010020 [5A03] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A03Value] NrOfEntries=1 1=0x40010020 [5A04] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A04Value] NrOfEntries=1 1=0x40010020 [5A05] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A05Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5C02] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000104 [5C03] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000105 [5C04] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000106 [5C05] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000107 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 [5E02] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E02Value] NrOfEntries=1 1=0x40000020 [5E03] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E03Value] NrOfEntries=1 1=0x40000020 [5E04] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E04Value] NrOfEntries=1 1=0x40000020 [5E05] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E05Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_proxy_driver/config/nmt_test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=28 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1600 15=0x1800 16=0x1A00 17=0x1F25 18=0x1F55 19=0x1F80 20=0x1F81 21=0x1F82 22=0x1F84 23=0x1F85 24=0x1F86 25=0x1F87 26=0x1F88 27=0x1F89 28=0x1F8A [ManufacturerObjects] SupportedObjects=6 1=0x2000 2=0x2200 3=0x5800 4=0x5A00 5=0x5C00 6=0x5E00 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=1 2=0x00000082 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=1 2=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=1 2=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=motioncontroller_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=motioncontroller_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_proxy_driver/config/pdo_test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=28 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1600 15=0x1800 16=0x1A00 17=0x1F25 18=0x1F55 19=0x1F80 20=0x1F81 21=0x1F82 22=0x1F84 23=0x1F85 24=0x1F86 25=0x1F87 26=0x1F88 27=0x1F89 28=0x1F8A [ManufacturerObjects] SupportedObjects=6 1=0x2000 2=0x2200 3=0x5800 4=0x5A00 5=0x5C00 6=0x5E00 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=1 2=0x00000082 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=1 2=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=1 2=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=motioncontroller_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=motioncontroller_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_proxy_driver/config/sdo_test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=28 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1600 15=0x1800 16=0x1A00 17=0x1F25 18=0x1F55 19=0x1F80 20=0x1F81 21=0x1F82 22=0x1F84 23=0x1F85 24=0x1F86 25=0x1F87 26=0x1F88 27=0x1F89 28=0x1F8A [ManufacturerObjects] SupportedObjects=6 1=0x2000 2=0x2200 3=0x5800 4=0x5A00 5=0x5C00 6=0x5E00 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=1 2=0x00000082 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=1 2=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=1 2=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=motioncontroller_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=motioncontroller_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_proxy_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ #define CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ #include "canopen_core/driver_node.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" namespace ros2_canopen { /** * @brief Lifecycle Proxy Driver * * A very basic driver without any functionality. * */ class LifecycleProxyDriver : public ros2_canopen::LifecycleCanopenDriver { std::shared_ptr> node_canopen_proxy_driver_; public: LifecycleProxyDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); virtual bool reset_node_nmt_command() { return node_canopen_proxy_driver_->reset_node_nmt_command(); } virtual bool start_node_nmt_command() { return node_canopen_proxy_driver_->start_node_nmt_command(); } virtual bool tpdo_transmit(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->tpdo_transmit(data); } virtual bool sdo_write(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->sdo_write(data); } virtual bool sdo_read(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->sdo_read(data); } void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_proxy_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_proxy_driver_->register_rpdo_cb(rpdo_cb); } }; } // namespace ros2_canopen #endif // CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ ================================================ FILE: canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_PROXY_DRIVER #define NODE_CANOPEN_PROXY_DRIVER #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" namespace ros2_canopen { namespace node_interfaces { template class NodeCanopenProxyDriver : public NodeCanopenBaseDriver { static_assert( std::is_base_of::value || std::is_base_of::value, "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); protected: rclcpp::Publisher::SharedPtr nmt_state_publisher; rclcpp::Publisher::SharedPtr rpdo_publisher; rclcpp::Subscription::SharedPtr tpdo_subscriber; rclcpp::Service::SharedPtr nmt_state_reset_service; rclcpp::Service::SharedPtr nmt_state_start_service; rclcpp::Service::SharedPtr sdo_read_service; rclcpp::Service::SharedPtr sdo_write_service; std::mutex sdo_mtex; virtual void on_nmt(canopen::NmtState nmt_state) override; virtual void on_rpdo(COData data) override; virtual void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg); virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override; void on_nmt_state_reset( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response); void on_nmt_state_start( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response); void on_sdo_read( const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response); void on_sdo_write( const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response); public: NodeCanopenProxyDriver(NODETYPE * node); virtual void init(bool called_from_base) override; virtual bool reset_node_nmt_command(); virtual bool start_node_nmt_command(); virtual bool tpdo_transmit(COData & data); virtual bool sdo_write(COData & data); virtual bool sdo_read(COData & data); }; } // namespace node_interfaces } // namespace ros2_canopen #endif ================================================ FILE: canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ #define NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ #include "canopen_core/driver_error.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" using namespace ros2_canopen::node_interfaces; template NodeCanopenProxyDriver::NodeCanopenProxyDriver(NODETYPE * node) : ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node) { } template void NodeCanopenProxyDriver::init(bool called_from_base) { RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented."); } template <> void NodeCanopenProxyDriver::init(bool called_from_base) { nmt_state_publisher = this->node_->create_publisher( std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10); tpdo_subscriber = this->node_->create_subscription( std::string(this->node_->get_name()).append("/tpdo").c_str(), 10, std::bind(&NodeCanopenProxyDriver::on_tpdo, this, std::placeholders::_1)); rpdo_publisher = this->node_->create_publisher( std::string(this->node_->get_name()).append("/rpdo").c_str(), 10); nmt_state_reset_service = this->node_->create_service( std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(), std::bind( &NodeCanopenProxyDriver::on_nmt_state_reset, this, std::placeholders::_1, std::placeholders::_2)); nmt_state_start_service = this->node_->create_service( std::string(this->node_->get_name()).append("/nmt_start_node").c_str(), std::bind( &NodeCanopenProxyDriver::on_nmt_state_start, this, std::placeholders::_1, std::placeholders::_2)); sdo_read_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_read").c_str(), std::bind( &NodeCanopenProxyDriver::on_sdo_read, this, std::placeholders::_1, std::placeholders::_2)); sdo_write_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_write").c_str(), std::bind( &NodeCanopenProxyDriver::on_sdo_write, this, std::placeholders::_1, std::placeholders::_2)); } template <> void NodeCanopenProxyDriver::init(bool called_from_base) { nmt_state_publisher = this->node_->create_publisher( std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10); tpdo_subscriber = this->node_->create_subscription( std::string(this->node_->get_name()).append("/tpdo").c_str(), 10, std::bind( &NodeCanopenProxyDriver::on_tpdo, this, std::placeholders::_1)); rpdo_publisher = this->node_->create_publisher( std::string(this->node_->get_name()).append("/rpdo").c_str(), 10); nmt_state_reset_service = this->node_->create_service( std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(), std::bind( &NodeCanopenProxyDriver::on_nmt_state_reset, this, std::placeholders::_1, std::placeholders::_2)); nmt_state_start_service = this->node_->create_service( std::string(this->node_->get_name()).append("/nmt_start_node").c_str(), std::bind( &NodeCanopenProxyDriver::on_nmt_state_start, this, std::placeholders::_1, std::placeholders::_2)); sdo_read_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_read").c_str(), std::bind( &NodeCanopenProxyDriver::on_sdo_read, this, std::placeholders::_1, std::placeholders::_2)); sdo_write_service = this->node_->create_service( std::string(this->node_->get_name()).append("/sdo_write").c_str(), std::bind( &NodeCanopenProxyDriver::on_sdo_write, this, std::placeholders::_1, std::placeholders::_2)); } template void NodeCanopenProxyDriver::on_nmt(canopen::NmtState nmt_state) { if (this->activated_.load()) { auto message = std_msgs::msg::String(); switch (nmt_state) { case canopen::NmtState::BOOTUP: message.data = "BOOTUP"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT bootup", "NMT", "BOOTUP"); break; case canopen::NmtState::PREOP: message.data = "PREOP"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT preop", "NMT", "PREOP"); break; case canopen::NmtState::RESET_COMM: message.data = "RESET_COMM"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset comm", "NMT", "RESET_COMM"); break; case canopen::NmtState::RESET_NODE: message.data = "RESET_NODE"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset node", "NMT", "RESET_NODE"); break; case canopen::NmtState::START: message.data = "START"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT start", "NMT", "START"); break; case canopen::NmtState::STOP: message.data = "STOP"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT stop", "NMT", "STOP"); break; case canopen::NmtState::TOGGLE: message.data = "TOGGLE"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT toggle", "NMT", "TOGGLE"); break; default: RCLCPP_ERROR(this->node_->get_logger(), "Unknown NMT State."); message.data = "ERROR"; this->diagnostic_collector_->updateAll( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NMT unknown state", "NMT", "ERROR"); break; } RCLCPP_INFO( this->node_->get_logger(), "Slave 0x%X: Switched NMT state to %s", this->lely_driver_->get_id(), message.data.c_str()); nmt_state_publisher->publish(message); } } template void NodeCanopenProxyDriver::on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg) { ros2_canopen::COData data = {msg->index, msg->subindex, msg->data}; if (!tpdo_transmit(data)) { RCLCPP_ERROR(this->node_->get_logger(), "Could transmit PDO because driver not activated."); } } template bool NodeCanopenProxyDriver::tpdo_transmit(ros2_canopen::COData & data) { if (this->activated_.load()) { RCLCPP_DEBUG( this->node_->get_logger(), "Node ID 0x%X: Transmit PDO index %x, subindex %hhu, data %d", this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_); this->lely_driver_->tpdo_transmit(data); return true; } return false; } template void NodeCanopenProxyDriver::on_rpdo(ros2_canopen::COData d) { if (this->activated_.load()) { // RCLCPP_DEBUG( // this->node_->get_logger(), "Node ID 0x%X: Received PDO index %#04x, subindex %hhu, data // %x", this->lely_driver_->get_id(), d.index_, d.subindex_, d.data_); auto message = canopen_interfaces::msg::COData(); message.index = d.index_; message.subindex = d.subindex_; message.data = d.data_; rpdo_publisher->publish(message); } } template void NodeCanopenProxyDriver::on_nmt_state_reset( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { response->success = reset_node_nmt_command(); } template bool NodeCanopenProxyDriver::reset_node_nmt_command() { if (this->activated_.load()) { this->lely_driver_->nmt_command(canopen::NmtCommand::RESET_NODE); return true; } RCLCPP_ERROR( this->node_->get_logger(), "Could not reset device via NMT because driver not activated."); return false; } template void NodeCanopenProxyDriver::on_nmt_state_start( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { response->success = start_node_nmt_command(); } template bool NodeCanopenProxyDriver::start_node_nmt_command() { if (this->activated_.load()) { this->lely_driver_->nmt_command(canopen::NmtCommand::START); return true; } RCLCPP_ERROR( this->node_->get_logger(), "Could not start device via NMT because driver not activated."); return false; } template void NodeCanopenProxyDriver::on_sdo_read( const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response) { ros2_canopen::COData data = {request->index, request->subindex, 0U}; response->success = sdo_read(data); response->data = data.data_; } template bool NodeCanopenProxyDriver::sdo_read(ros2_canopen::COData & data) { if (this->activated_.load()) { RCLCPP_INFO( this->node_->get_logger(), "Slave 0x%X: SDO Read Call index=0x%X subindex=%hhu", this->lely_driver_->get_id(), data.index_, data.subindex_); // Only allow one SDO request concurrently std::scoped_lock lk(sdo_mtex); // Send read request auto f = this->lely_driver_->async_sdo_read(data); // Wait for response f.wait(); // Process response try { data.data_ = f.get().data_; } catch (std::exception & e) { RCLCPP_ERROR(this->node_->get_logger(), e.what()); return false; } return true; } RCLCPP_ERROR(this->node_->get_logger(), "Could not read from SDO because driver not activated."); return false; } template void NodeCanopenProxyDriver::on_sdo_write( const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response) { ros2_canopen::COData data = {request->index, request->subindex, request->data}; response->success = sdo_write(data); } template bool NodeCanopenProxyDriver::sdo_write(ros2_canopen::COData & data) { if (this->activated_.load()) { RCLCPP_INFO( this->node_->get_logger(), "Slave 0x%X: SDO Write Call index=0x%X subindex=%hhu data=%u", this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_); // Only allow one SDO request concurrently std::scoped_lock lk(sdo_mtex); // Send write request auto f = this->lely_driver_->async_sdo_write(data); // Wait for request to complete f.wait(); // Process response try { return f.get(); } catch (std::exception & e) { RCLCPP_ERROR(this->node_->get_logger(), e.what()); return false; } return true; } RCLCPP_ERROR(this->node_->get_logger(), "Could not write to SDO because driver not activated."); return false; } template void NodeCanopenProxyDriver::diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage()); stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE")); stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT")); stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY")); } #endif ================================================ FILE: canopen_proxy_driver/include/canopen_proxy_driver/proxy_driver.hpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ #define CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ #include "canopen_core/driver_node.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" namespace ros2_canopen { /** * @brief Abstract Class for a CANopen Device Node * * This class provides the base functionality for creating a * CANopen device node. It provides callbacks for nmt and rpdo. */ class ProxyDriver : public ros2_canopen::CanopenDriver { std::shared_ptr> node_canopen_proxy_driver_; public: ProxyDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); virtual bool reset_node_nmt_command() { return node_canopen_proxy_driver_->reset_node_nmt_command(); } virtual bool start_node_nmt_command() { return node_canopen_proxy_driver_->start_node_nmt_command(); } virtual bool tpdo_transmit(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->tpdo_transmit(data); } virtual bool sdo_write(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->sdo_write(data); } virtual bool sdo_read(ros2_canopen::COData & data) { return node_canopen_proxy_driver_->sdo_read(data); } void register_nmt_state_cb(std::function nmt_state_cb) { node_canopen_proxy_driver_->register_nmt_state_cb(nmt_state_cb); } void register_rpdo_cb(std::function rpdo_cb) { node_canopen_proxy_driver_->register_rpdo_cb(rpdo_cb); } void register_emcy_cb(std::function emcy_cb) { node_canopen_proxy_driver_->register_emcy_cb(emcy_cb); } }; } // namespace ros2_canopen #endif // CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ ================================================ FILE: canopen_proxy_driver/include/canopen_proxy_driver/visibility_control.h ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_PROXY_DRIVER__VISIBILITY_CONTROL_H_ #define CANOPEN_PROXY_DRIVER__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_PROXY_DRIVER_EXPORT __attribute__((dllexport)) #define CANOPEN_PROXY_DRIVER_IMPORT __attribute__((dllimport)) #else #define CANOPEN_PROXY_DRIVER_EXPORT __declspec(dllexport) #define CANOPEN_PROXY_DRIVER_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY #define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_EXPORT #else #define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_IMPORT #endif #define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE CANOPEN_PROXY_DRIVER_PUBLIC #define CANOPEN_PROXY_DRIVER_LOCAL #else #define CANOPEN_PROXY_DRIVER_EXPORT __attribute__((visibility("default"))) #define CANOPEN_PROXY_DRIVER_IMPORT #if __GNUC__ >= 4 #define CANOPEN_PROXY_DRIVER_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_PROXY_DRIVER_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_PROXY_DRIVER_PUBLIC #define CANOPEN_PROXY_DRIVER_LOCAL #endif #define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_PROXY_DRIVER__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_proxy_driver/package.xml ================================================ canopen_proxy_driver 0.3.2 Simple proxy driver for the ros2_canopen stack Christoph Hellmann Santos Apache-2.0 ament_cmake_ros canopen_base_driver canopen_core canopen_interfaces rclcpp rclcpp_components rclcpp_lifecycle ament_lint_auto ament_cmake ================================================ FILE: canopen_proxy_driver/readme.md ================================================ # canopen_ros_proxy_driver ## Testing ``` colcon test --packages-select canopen_proxy_driver --event-handlers desktop_notification+ status+ summary- console_direct+ terminal_title+ ``` ================================================ FILE: canopen_proxy_driver/src/lifecycle_proxy_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_proxy_driver/lifecycle_proxy_driver.hpp" using namespace ros2_canopen; LifecycleProxyDriver::LifecycleProxyDriver(rclcpp::NodeOptions node_options) : LifecycleCanopenDriver(node_options) { node_canopen_proxy_driver_ = std::make_shared>( this); node_canopen_driver_ = std::static_pointer_cast( node_canopen_proxy_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleProxyDriver) ================================================ FILE: canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp" using namespace ros2_canopen::node_interfaces; template class ros2_canopen::node_interfaces::NodeCanopenProxyDriver; template class ros2_canopen::node_interfaces::NodeCanopenProxyDriver< rclcpp_lifecycle::LifecycleNode>; ================================================ FILE: canopen_proxy_driver/src/proxy_driver.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_proxy_driver/proxy_driver.hpp" using namespace ros2_canopen; ProxyDriver::ProxyDriver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) { node_canopen_proxy_driver_ = std::make_shared>(this); node_canopen_driver_ = std::static_pointer_cast( node_canopen_proxy_driver_); } #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::ProxyDriver) ================================================ FILE: canopen_proxy_driver/test/CMakeLists.txt ================================================ ament_add_gtest(test_node_interface test_node_interface.cpp ) target_include_directories(test_node_interface PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_node_interface ${canopen_interfaces_TARGETS} canopen_base_driver::base_driver canopen_base_driver::lely_driver_bridge canopen_base_driver::lifecycle_base_driver canopen_base_driver::node_canopen_base_driver canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master node_canopen_proxy_driver rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gtest(test_driver_component test_driver_component.cpp ) target_include_directories(test_driver_component PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ ) target_link_libraries(test_driver_component ${canopen_interfaces_TARGETS} canopen_base_driver::base_driver canopen_base_driver::lely_driver_bridge canopen_base_driver::lifecycle_base_driver canopen_base_driver::node_canopen_base_driver canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) ================================================ FILE: canopen_proxy_driver/test/master.dcf ================================================ [DeviceComissioning] NodeID=1 NodeName= NodeRefd= Baudrate=1000 NetNumber=1 NetworkName= NetRefd= CANopenManager=1 LSS_SerialNumber=0x00000000 [DeviceInfo] VendorName= VendorNumber=0x00000000 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=1 SimpleBootUpSlave=0 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=2 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=32 1=0x1003 2=0x1005 3=0x1006 4=0x1007 5=0x1014 6=0x1015 7=0x1016 8=0x1017 9=0x1019 10=0x1028 11=0x1029 12=0x102A 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1A00 20=0x1A01 21=0x1F25 22=0x1F55 23=0x1F80 24=0x1F81 25=0x1F82 26=0x1F84 27=0x1F85 28=0x1F86 29=0x1F87 30=0x1F88 31=0x1F89 32=0x1F8A [ManufacturerObjects] SupportedObjects=12 1=0x2000 2=0x2001 3=0x2200 4=0x2201 5=0x5800 6=0x5801 7=0x5A00 8=0x5A01 9=0x5C00 10=0x5C01 11=0x5E00 12=0x5E01 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x40000080 [1006] ParameterName=Communication cycle period DataType=0x0007 AccessType=rw DefaultValue=0 [1007] ParameterName=Synchronous window length DataType=0x0007 AccessType=rw DefaultValue=0 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1016Value] NrOfEntries=0 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw DefaultValue=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1019] ParameterName=Synchronous counter overflow value DataType=0x0005 AccessType=rw DefaultValue=0 [1028] ParameterName=Emergency consumer object ObjectType=0x08 DataType=0x0007 AccessType=rw DefaultValue=0x80000000 CompactSubObj=127 [1028Value] NrOfEntries=2 2=0x00000082 3=0x00000083 [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=254 [1029Value] NrOfEntries=1 1=0x00 [102A] ParameterName=NMT inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000182 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1401] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1401sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1401sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000183 [1401sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1401sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1401sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1401sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw DefaultValue=0 [1600] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x20000120 [1601] ParameterName=RPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1601Value] NrOfEntries=1 1=0x20010120 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000202 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1801] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=0x00000203 [1801sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer DataType=0x0006 AccessType=rw DefaultValue=0 [1801sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw DefaultValue=0 [1A00] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x22000120 [1A01] ParameterName=TPDO mapping parameter ObjectType=0x09 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x22010120 [1F25] ParameterName=Configuration request ObjectType=0x08 DataType=0x0005 AccessType=wo CompactSubObj=127 [1F55] ParameterName=Expected software identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F80] ParameterName=NMT startup DataType=0x0007 AccessType=rw DefaultValue=0x00000001 [1F81] ParameterName=NMT slave assignment ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F81Value] NrOfEntries=2 2=0x00000005 3=0x00000005 [1F82] ParameterName=Request NMT ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F84] ParameterName=Device type identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F84Value] NrOfEntries=0 [1F85] ParameterName=Vendor identification ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F85Value] NrOfEntries=2 2=0x00000360 3=0x00000360 [1F86] ParameterName=Product code ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F86Value] NrOfEntries=0 [1F87] ParameterName=Revision_number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F88] ParameterName=Serial number ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=127 [1F89] ParameterName=Boot time DataType=0x0007 AccessType=rw DefaultValue=0 [1F8A] ParameterName=Restore configuration ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=127 [1F8AValue] NrOfEntries=0 [2000] SubNumber=2 ParameterName=Mapped application objects for RPDO 1 ObjectType=0x09 [2000sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2000sub1] ParameterName=proxy_device_1: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2001] SubNumber=2 ParameterName=Mapped application objects for RPDO 2 ObjectType=0x09 [2001sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2001sub1] ParameterName=proxy_device_2: UNSIGNED32 sent from slave DataType=0x0007 AccessType=rww PDOMapping=1 [2200] SubNumber=2 ParameterName=Mapped application objects for TPDO 1 ObjectType=0x09 [2200sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2200sub1] ParameterName=proxy_device_1: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [2201] SubNumber=2 ParameterName=Mapped application objects for TPDO 2 ObjectType=0x09 [2201sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=1 [2201sub1] ParameterName=proxy_device_2: UNSIGNED32 received by slave DataType=0x0007 AccessType=rwr PDOMapping=1 [5800] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5801] ParameterName=Remote TPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5A00] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A00Value] NrOfEntries=1 1=0x40010020 [5A01] ParameterName=Remote TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5A01Value] NrOfEntries=1 1=0x40010020 [5C00] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000102 [5C01] ParameterName=Remote RPDO number and node-ID DataType=0x0007 AccessType=rw DefaultValue=0x00000103 [5E00] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E00Value] NrOfEntries=1 1=0x40000020 [5E01] ParameterName=Remote RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [5E01Value] NrOfEntries=1 1=0x40000020 ================================================ FILE: canopen_proxy_driver/test/test_driver_component.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include #include #include #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "gtest/gtest.h" using namespace rclcpp_components; TEST(ComponentLoad, test_load_component_1) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_proxy_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[0]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } TEST(ComponentLoad, test_load_component_2) { rclcpp::init(0, nullptr); auto exec = std::make_shared(); auto manager = std::make_shared(exec); std::vector resources = manager->get_component_resources("canopen_proxy_driver"); EXPECT_EQ(2u, resources.size()); auto factory = manager->create_component_factory(resources[1]); auto instance_wrapper = factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); rclcpp::shutdown(); } ================================================ FILE: canopen_proxy_driver/test/test_node_interface.cpp ================================================ // Copyright 2022 Christoph Hellmann Santos // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" #include "gtest/gtest.h" TEST(NodeCanopenProxyDriver, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp::Node * node = new rclcpp::Node("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " "\"simple.eds\"\ndcf_path: \"\"\n"); node->set_parameter(container_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); // Can't activate as master cannot be set. EXPECT_ANY_THROW(iface->activate()); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) { rclcpp::init(0, nullptr); rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); auto interface = new ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node); auto exec = std::make_shared(); exec->add_node(node->get_node_base_interface()); std::thread spinner = std::thread([exec] { exec->spin(); }); auto iface = static_cast(interface); EXPECT_NO_THROW(iface->init()); rclcpp::Parameter container_name("container_name", "none"); rclcpp::Parameter node_id("node_id", 1); rclcpp::Parameter timeout("non_transmit_timeout", 100); rclcpp::Parameter config( "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " "\"simple.eds\"\ndcf_path: \"\"\n"); node->set_parameter(container_name); node->set_parameter(node_id); node->set_parameter(timeout); node->set_parameter(config); EXPECT_NO_THROW(iface->configure()); // Can't activate as master cannot be set. EXPECT_ANY_THROW(iface->activate()); rclcpp::shutdown(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); if (spinner.joinable()) { spinner.join(); } } ================================================ FILE: canopen_ros2_control/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * add pdo 6077 torque actual value to the joint state interface as effort (`#316 `_) Co-authored-by: Vishnuprasad Prachandabhanu <32260301+ipa-vsp@users.noreply.github.com> * Refactor on_init method for improved readability and consistency * Fix deprecated hardware_interface API (`#386 `_) * Fix typos in warning messages and comments for clarity * `#379 `_: Fix data conversion in the Lely Bridge to enable more data types and proper handling of Emcy in ros2_control * Return error on Emcy. * Add correct data conversion for Emcy. * Fixed types handling in canopen_ros2_control. * Optimize debug output. * Fixed sending values. * Contributors: Christoph Fröhlich, Dr. Denis Stogl, Vishnuprasad Prachandabhanu, synsi23b 0.3.1 (2025-06-23) ------------------ * Fixing ID type in storage of ros2_control system. * Contributors: Dr. Denis, Gerry Salinas, Vishnuprasad Prachandabhanu 0.3.0 (2024-12-12) ------------------ * pre-commit fix * impl operation mode * Add cyclic torque mode to cia402 driver and robot system controller (`#293 `_) * Add base functions for switching to cyclic torque mode * Add cyclic torque mode as effort interface to robot_system controller * Add documentation about cyclic torque mode. Co-authored-by: Christoph Hellmann Santos * Fix clang format * Update canopen_system.hpp * Add pdo mapping support * Fix clang format * Fix pre-commit * Periodic messages sent, but not received properly. * Fix the bug that the rpdo queue keeps poping although it is empty. * Use proper function to get rpdo data * Fix bug in state interface indexing.. * WIP: Extend the rpdo to have a queue (FIFO) to save the values. The read function take the latest value out of the queue and assign to the system interface. Need tests. 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Contributors: ipa-vsp 0.2.9 (2024-04-16) ------------------ 0.2.8 (2024-01-19) ------------------ * Update robot_system.cpp (`#168 `_) * Contributors: Christoph Hellmann Santos 0.2.7 (2023-06-30) ------------------ * Correct Proxy controller after changes and update tests. * Contributors: Dr. Denis, Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ * Solve buildfarm issues * Contributors: Christoph Hellmann Santos 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Use consistenlty (uppercase) HEX output for NodeID and Index. * Contributors: Christoph Hellmann Santos, Denis Štogl 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu, livanov93 ================================================ FILE: canopen_ros2_control/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_ros2_control) 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(canopen_402_driver REQUIRED) find_package(canopen_core REQUIRED) find_package(canopen_proxy_driver REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(ros2_control_test_assets) add_library( canopen_ros2_control SHARED src/canopen_system.cpp src/cia402_system.cpp src/robot_system.cpp ) target_compile_features(canopen_ros2_control PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(canopen_ros2_control PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(canopen_ros2_control PUBLIC include) target_link_libraries(canopen_ros2_control PUBLIC canopen_402_driver::cia402_driver canopen_402_driver::lely_motion_controller_bridge canopen_402_driver::lifecycle_cia402_driver canopen_402_driver::node_canopen_cia402_driver canopen_core::device_container canopen_core::node_canopen_driver canopen_core::node_canopen_master canopen_proxy_driver::lifecycle_proxy_driver canopen_proxy_driver::node_canopen_proxy_driver canopen_proxy_driver::proxy_driver hardware_interface::hardware_interface hardware_interface::mock_components pluginlib::pluginlib rclcpp::rclcpp rclcpp_components::component rclcpp_components::component_manager rclcpp_lifecycle::rclcpp_lifecycle ) # prevent pluginlib from using boost target_compile_definitions(canopen_ros2_control PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(hardware_interface canopen_ros2_control.xml) install( TARGETS canopen_ros2_control RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) install( DIRECTORY include/ DESTINATION include ) if(BUILD_TESTING) #find_package(ament_cmake_gmock REQUIRED) #find_package(ros2_control_test_assets REQUIRED) #ament_add_gmock(test_canopen_system test/test_canopen_system.cpp) #target_include_directories(test_canopen_system PRIVATE include) #target_link_libraries(test_canopen_system ... endif() ament_export_include_directories( include ) ament_export_libraries( canopen_ros2_control ) ament_export_dependencies( canopen_402_driver canopen_core canopen_proxy_driver hardware_interface pluginlib rclcpp rclcpp_components rclcpp_lifecycle ) ament_package() ================================================ FILE: canopen_ros2_control/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_ros2_control/canopen_ros2_control.xml ================================================ ros2_control hardware interface. ros2_control hardware interface. ros2_control hardware interface. ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. //---------------------------------------------------------------------- /*!\file * * \author Lovro Ivanov lovro.ivanov@gmail.com * \date 2022-06-29 * */ //---------------------------------------------------------------------- #ifndef CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ #define CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ #include #include #include #include #include #include #include "canopen_core/device_container.hpp" #include "canopen_proxy_driver/proxy_driver.hpp" #include "canopen_ros2_control/visibility_control.h" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" namespace canopen_ros2_control { template auto makeMemcpyCaster(const SourceType & source) { return [&source]() -> TargetType { TargetType target; std::memcpy(&target, &source, sizeof(TargetType)); return target; }; } const std::vector SUPPORTED_TYPES = {"bool", "int8_t", "uint8_t", "int16_t", "uint16_t", "int32_t", "uint32_t"}; // needed auxiliary struct for ros2 control double registration struct Ros2ControlCOData { // TODO(Dr.Denis): rename original data to canopen data // Soon we can drop this as ros2_control support variants - we have to add support for all this // types ros2_canopen::COData original_data; double index; // cast to uint16_t double subindex; // cast to uint8_t double data; // cast to uint32_t std::string co_type = "int32_t"; // use int32_t as default void set_co_data_type(const std::string & type) { if ( type.empty() || std::find(SUPPORTED_TYPES.begin(), SUPPORTED_TYPES.end(), type) != SUPPORTED_TYPES.end()) { co_type = type; } else { RCLCPP_WARN( rclcpp::get_logger("rclcpp"), "Type '%s' empty or not yet supported. Using 'int32_t' as default to cast directly to " "double. This might cause erroneous data! Please contribute or by maintainers a cookie and " "hope they implement it for you!", type.c_str()); co_type = "int32_t"; } } }; struct RORos2ControlCOData : public Ros2ControlCOData { void set_data(ros2_canopen::COData d) { original_data = d; } void prepare_data() { index = static_cast(original_data.index_); subindex = static_cast(original_data.subindex_); if (co_type == "bool") { bool bool_data; std::memcpy(&bool_data, &original_data.data_, sizeof(bool)); data = static_cast(bool_data); } else if (co_type == "int8_t") { int8_t int8_data; std::memcpy(&int8_data, &original_data.data_, sizeof(int8_t)); data = static_cast(int8_data); } else if (co_type == "uint8_t") { uint8_t uint8_data; std::memcpy(&uint8_data, &original_data.data_, sizeof(uint8_t)); data = static_cast(uint8_data); } else if (co_type == "int16_t") { int16_t int16_data; std::memcpy(&int16_data, &original_data.data_, sizeof(int16_t)); data = static_cast(int16_data); } else if (co_type == "uint16_t") { uint16_t uint16_data; std::memcpy(&uint16_data, &original_data.data_, sizeof(uint16_t)); data = static_cast(uint16_data); } else if (co_type == "int32_t") { int32_t int32_data; std::memcpy(&int32_data, &original_data.data_, sizeof(int32_t)); data = static_cast(int32_data); } else if (co_type == "uint32_t") { uint32_t uint32_data; std::memcpy(&uint32_data, &original_data.data_, sizeof(uint32_t)); data = static_cast(uint32_data); } else { RCLCPP_WARN( rclcpp::get_logger("rclcpp"), "Type '%s' not yet supported. Trying to cast directly to double. This might cause " "erroneous data! Please contribute or by maintainers a cookie and hope they implement it " "for you!", co_type.c_str()); data = static_cast(original_data.data_); } } }; struct WORos2ControlCoData : public Ros2ControlCOData { WORos2ControlCoData() : one_shot(std::numeric_limits::quiet_NaN()) {} // needed internally for write-only data double one_shot; bool write_command() { bool ret_val; // store ret value ret_val = !std::isnan(one_shot); // reset the existing active command if one exists one_shot = std::numeric_limits::quiet_NaN(); return ret_val; } void prepare_data() { original_data.index_ = static_cast(index); original_data.subindex_ = static_cast(subindex); if (co_type == "bool") { bool bool_data = static_cast(data); std::memcpy(&original_data.data_, &bool_data, sizeof(bool)); } else if (co_type == "int8_t") { int8_t int8_data = static_cast(data); std::memcpy(&original_data.data_, &int8_data, sizeof(int8_t)); } else if (co_type == "uint8_t") { uint8_t uint8_data = static_cast(data); std::memcpy(&original_data.data_, &uint8_data, sizeof(uint8_t)); } else if (co_type == "int16_t") { int16_t int16_data = static_cast(data); std::memcpy(&original_data.data_, &int16_data, sizeof(int16_t)); } else if (co_type == "uint16_t") { uint16_t uint16_data = static_cast(data); std::memcpy(&original_data.data_, &uint16_data, sizeof(uint16_t)); } else if (co_type == "int32_t") { int32_t int32_data = static_cast(data); std::memcpy(&original_data.data_, &int32_data, sizeof(int32_t)); } else if (co_type == "uint32_t") { uint32_t uint32_data = static_cast(data); std::memcpy(&original_data.data_, &uint32_data, sizeof(uint32_t)); } else { RCLCPP_WARN( rclcpp::get_logger("rclcpp"), "Type '%s' not yet supported. Trying to cast directly to uint32_t. This might cause " "erroneous data! Please contribute or by maintainers a cookie and hope they implement it " "for you!", co_type.c_str()); original_data.data_ = static_cast(data); } } }; struct Ros2ControlEmcyData { Ros2ControlEmcyData() : error_code(0), error_register(0), manufacturer_error_code1(0), manufacturer_error_code2(0), manufacturer_error_code3(0), manufacturer_error_code4(0), manufacturer_error_code5(0) { } void set_emcy(ros2_canopen::COEmcy e) { original_emcy = e; uint16_t eec_data; std::memcpy(&eec_data, &e.eec, sizeof(uint16_t)); error_code = static_cast(eec_data); uint8_t er_data; std::memcpy(&er_data, &e.er, sizeof(uint8_t)); error_register = static_cast(er_data); uint16_t msef_data; std::memcpy(&msef_data, &e.msef[0], sizeof(uint16_t)); manufacturer_error_code1 = static_cast(msef_data); std::memcpy(&msef_data, &e.msef[1], sizeof(uint16_t)); manufacturer_error_code2 = static_cast(msef_data); std::memcpy(&msef_data, &e.msef[2], sizeof(uint16_t)); manufacturer_error_code3 = static_cast(msef_data); std::memcpy(&msef_data, &e.msef[3], sizeof(uint16_t)); manufacturer_error_code4 = static_cast(msef_data); std::memcpy(&msef_data, &e.msef[4], sizeof(uint16_t)); manufacturer_error_code5 = static_cast(msef_data); } double error_code; // read-only double error_register; // read-only double manufacturer_error_code1; // read-only double manufacturer_error_code2; // read-only double manufacturer_error_code3; // read-only double manufacturer_error_code4; // read-only double manufacturer_error_code5; // read-only ros2_canopen::COEmcy original_emcy; // read-only }; struct Ros2ControlNmtState { Ros2ControlNmtState() : reset_ons(std::numeric_limits::quiet_NaN()), reset_fbk(std::numeric_limits::quiet_NaN()), start_ons(std::numeric_limits::quiet_NaN()), start_fbk(std::numeric_limits::quiet_NaN()) { } void set_state(canopen::NmtState s) { original_state = s; state = static_cast(s); } bool reset_command() { bool ret_val; // store ret value ret_val = !std::isnan(reset_ons); // reset the existing active command if one exists reset_ons = std::numeric_limits::quiet_NaN(); return ret_val; } bool start_command() { bool ret_val; // store ret value ret_val = !std::isnan(start_ons); // reset the existing active command if one exists start_ons = std::numeric_limits::quiet_NaN(); return ret_val; } canopen::NmtState original_state; double state; // read-only // basic commands double reset_ons; // write-only double reset_fbk; double start_ons; // write-only double start_fbk; }; struct pair_hash { template std::size_t operator()(const std::pair & pair) const { auto h1 = std::hash{}(pair.first); auto h2 = std::hash{}(pair.second); return h1 ^ h2; } }; struct CanopenNodeData { Ros2ControlNmtState nmt_state; // read-write RORos2ControlCOData rpdo_data; // read-only WORos2ControlCoData tpdo_data; // write-only Ros2ControlEmcyData emcy_data; // read-only WORos2ControlCoData rsdo; // write-only WORos2ControlCoData wsdo; // write-only using PDO_INDICES = std::pair; // Index, Subindex std::unordered_map rpdo_data_map; // kept for backward compoatibility - should b removed std::unordered_map rpdo_raw_data_map; // Push data to the queue - FIFO void set_rpdo_data(ros2_canopen::COData d) { rpdo_data.set_data(d); rpdo_data.prepare_data(); PDO_INDICES index_pair(d.index_, d.subindex_); // check if the index pair is already in the map if (rpdo_raw_data_map.find(index_pair) != rpdo_raw_data_map.end()) { // if it is, update the value rpdo_raw_data_map.at(index_pair) = d.data_; } else { // if it is not, add it to the map rpdo_raw_data_map.emplace(index_pair, d.data_); } // TODO(Dr.Denis): kept for backward compatibility - should be removed // check if the index pair is already in the map if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) { // if it is, update the value rpdo_data_map.at(index_pair) = rpdo_data.data; } else { // if it is not, add it to the map rpdo_data_map.emplace(index_pair, rpdo_data.data); } } uint32_t get_rpdo_raw_data(uint16_t index, uint8_t subindex) { PDO_INDICES index_pair(index, subindex); if (rpdo_raw_data_map.find(index_pair) != rpdo_raw_data_map.end()) { return rpdo_raw_data_map.at(index_pair); } else { // Log error // RCLCPP_WARN(kLogger, "The index pair (%u, %u) is not in the map", index, subindex); return std::numeric_limits::quiet_NaN(); } } // Pop data from the queue double get_rpdo_data(uint16_t index, uint8_t subindex) { PDO_INDICES index_pair(index, subindex); if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) { return rpdo_data_map[index_pair]; } else { // // Log error // RCLCPP_WARN(kLogger, "The index pair (%u, %u) is not in the map", index, subindex); return std::numeric_limits::quiet_NaN(); } } }; class CanopenSystem : public hardware_interface::SystemInterface { public: CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC CanopenSystem(); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC ~CanopenSystem(); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_state_interfaces() override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_command_interfaces() override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::shared_ptr device_container_; std::shared_ptr executor_; // can stuff std::map canopen_data_; // threads std::unique_ptr spin_thread_; std::unique_ptr init_thread_; void spin(); void clean(); private: void initDeviceContainer(); }; } // namespace canopen_ros2_control #endif // CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp ================================================ // Copyright (c) 2023, Fraunhofer IPA // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROL__CIA402_DATA_HPP_ #define CANOPEN_ROS2_CONTROL__CIA402_DATA_HPP_ #include #include #include #include #include "canopen_402_driver/base.hpp" #include "canopen_402_driver/cia402_driver.hpp" #include "canopen_ros2_control/helpers.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" using namespace ros2_canopen; namespace canopen_ros2_control { struct Cia402Data { uint8_t node_id; uint8_t channel = 0; // Channel for multi-axis support (0 for single-axis, 1+ for multi-axis) std::string joint_name; std::shared_ptr driver; std::map command_interface_to_operation_mode; std::vector interfaces; std::vector interfaces_to_start; std::vector interfaces_to_running; std::vector interfaces_to_stop; YAML::Node config; // FROM MOTOR double actual_position = std::numeric_limits::quiet_NaN(); double actual_velocity = std::numeric_limits::quiet_NaN(); double actual_effort = std::numeric_limits::quiet_NaN(); // TO MOTOR double target_position = std::numeric_limits::quiet_NaN(); double target_velocity = std::numeric_limits::quiet_NaN(); double target_torque = std::numeric_limits::quiet_NaN(); bool init_data(hardware_interface::ComponentInfo & joint, std::string device_dump) { joint_name = joint.name; auto config = YAML::Load(device_dump); if (!config["node_id"]) { RCLCPP_ERROR(rclcpp::get_logger(joint_name), "No node id for '%s'", joint.name.c_str()); return false; } node_id = config["node_id"].as(); // Get channel parameter (defaults to 0 for backward compatibility) if (config["channel"]) { channel = config["channel"].as(); RCLCPP_INFO( rclcpp::get_logger(joint_name), "Node id for '%s' is '%u', channel is '%u'", joint.name.c_str(), node_id, channel); } else { RCLCPP_ERROR( rclcpp::get_logger(joint_name), "Node id for '%s' is '%u'", joint.name.c_str(), node_id); } if (config["position_mode"]) { auto position_mode = (ros2_canopen::MotorBase::OperationMode)config["position_mode"].as(); RCLCPP_INFO( rclcpp::get_logger(joint_name), "Registered position_mode '%u' for '%s'", position_mode, joint.name.c_str()); command_interface_to_operation_mode.emplace( std::pair(joint.name + "/" + "position", position_mode)); } if (config["velocity_mode"]) { auto velocity_mode = (ros2_canopen::MotorBase::OperationMode)config["velocity_mode"].as(); RCLCPP_INFO( rclcpp::get_logger(joint_name), "Registered velocity_mode '%u' for '%s'", velocity_mode, joint.name.c_str()); command_interface_to_operation_mode.emplace( std::pair(joint.name + "/" + "velocity", velocity_mode)); } if (config["effort_mode"]) { auto effort_mode = (ros2_canopen::MotorBase::OperationMode)config["effort_mode"].as(); RCLCPP_INFO( rclcpp::get_logger(joint_name), "Registered effort_mode '%u' for '%s'", effort_mode, joint.name.c_str()); command_interface_to_operation_mode.emplace( std::pair(joint.name + "/" + "effort", effort_mode)); } return true; } void export_state_interface(std::vector & state_interfaces) { // actual position state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &actual_position)); // actual speed state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_VELOCITY, &actual_velocity)); // actual effort state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_EFFORT, &actual_effort)); } void export_command_interface( std::vector & command_interfaces) { if ( command_interface_to_operation_mode.find(joint_name + "/" + "position") != command_interface_to_operation_mode.end()) { // target position command_interfaces.emplace_back( joint_name, hardware_interface::HW_IF_POSITION, &target_position); interfaces.push_back(joint_name + "/" + "position"); } if ( command_interface_to_operation_mode.find(joint_name + "/" + "velocity") != command_interface_to_operation_mode.end()) { // target velocity command_interfaces.emplace_back( joint_name, hardware_interface::HW_IF_VELOCITY, &target_velocity); interfaces.push_back(joint_name + "/" + "velocity"); } if ( command_interface_to_operation_mode.find(joint_name + "/" + "effort") != command_interface_to_operation_mode.end()) { // target effort command_interfaces.emplace_back(joint_name, hardware_interface::HW_IF_EFFORT, &target_torque); interfaces.push_back(joint_name + "/" + "effort"); } } void read_state() { actual_position = driver->get_position(); actual_velocity = driver->get_speed(); actual_effort = driver->get_effort(); } void write_target() { const uint16_t & mode = driver->get_mode(); switch (mode) { case MotorBase::No_Mode: break; case MotorBase::Profiled_Position: case MotorBase::Cyclic_Synchronous_Position: case MotorBase::Interpolated_Position: driver->set_target(target_position); break; case MotorBase::Profiled_Velocity: case MotorBase::Cyclic_Synchronous_Velocity: driver->set_target(target_velocity); break; case MotorBase::Profiled_Torque: case MotorBase::Cyclic_Synchronous_Torque: driver->set_target(target_torque); break; default: RCLCPP_INFO(rclcpp::get_logger("robot_system_interface"), "Mode not supported"); } } bool perform_switch() { if (interfaces_to_start.size() > 1) { RCLCPP_ERROR( rclcpp::get_logger(joint_name), "Trying to start multiple command interfaces at once for joint '%s'", joint_name.c_str()); return false; } if (interfaces_to_start.size() == 0) { return true; } auto mode = command_interface_to_operation_mode[interfaces_to_start[0]]; interfaces_to_running.clear(); interfaces_to_running.push_back(interfaces_to_start[0]); interfaces_to_stop.clear(); interfaces_to_start.clear(); RCLCPP_INFO( rclcpp::get_logger(joint_name), "Switching to '%s' command mode with CIA402 operation mode '%u'", interfaces_to_running[0].c_str(), mode); return driver->set_operation_mode(mode); } bool check_id(uint8_t id) { if (node_id == id) { return true; } return false; } }; } // namespace canopen_ros2_control #endif ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp ================================================ // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. //---------------------------------------------------------------------- /*!\file * * \author Lovro Ivanov lovro.ivanov@gmail.com * \date 2022-08-01 * */ //---------------------------------------------------------------------- #ifndef CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ #define CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ #include "canopen_402_driver/cia402_driver.hpp" #include "canopen_ros2_control/canopen_system.hpp" constexpr double kResponseOk = 1.0; constexpr double kResponseFail = 0.0; namespace canopen_ros2_control { struct MotorTriggerCommand { double ons_cmd{std::numeric_limits::quiet_NaN()}; double resp{std::numeric_limits::quiet_NaN()}; bool is_commanded() { bool tmp = !std::isnan(ons_cmd); ons_cmd = std::numeric_limits::quiet_NaN(); return tmp; } void set_response(bool response) { resp = response ? kResponseOk : kResponseFail; } }; struct MotorTarget : public MotorTriggerCommand { double position_value; double velocity_value; double torque_value; }; struct MotorNodeData { // feedback double actual_position; double actual_speed; double actual_effort; // basic control MotorTriggerCommand init; MotorTriggerCommand halt; MotorTriggerCommand recover; // mode control MotorTriggerCommand position_mode; MotorTriggerCommand velocity_mode; MotorTriggerCommand cyclic_velocity_mode; MotorTriggerCommand cyclic_position_mode; MotorTriggerCommand torque_mode; MotorTriggerCommand interpolated_position_mode; // setpoint MotorTarget target; }; using namespace ros2_canopen; class Cia402System : public CanopenSystem { public: CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC Cia402System(); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC ~Cia402System() = default; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_state_interfaces(); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_command_interfaces(); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); protected: // can stuff // Map from (node_id, channel) to motor data for multi-channel support // channel 0 is used for single-channel devices for backward compatibility std::map, MotorNodeData> motor_data_; // Helper to get motor data key std::pair getMotorKey(uint node_id, uint channel = 0) const { return std::make_pair(node_id, channel); } private: void switchModes( uint id, uint channel, const std::shared_ptr & driver); void handleInit( uint id, uint channel, const std::shared_ptr & driver); void handleRecover( uint id, uint channel, const std::shared_ptr & driver); void handleHalt( uint id, uint channel, const std::shared_ptr & driver); void initDeviceContainer(); }; } // namespace canopen_ros2_control #endif // CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/helpers.hpp ================================================ // Copyright (c) 2023, Fraunhofer IPA // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROL__HELPER_HPP_ #define CANOPEN_ROS2_CONTROL__HELPER_HPP_ #include #include #include #include namespace canopen_ros2_control { /** * @brief Struct for storing the data necessary for a triggering command. * * @details * A trigger command from a controller will write to the command double. * The result of the trigger operation will be written to the response double. * A result of 1.0 means the operation was successful. * A result of 0.0 means the operation failed. * Only a trigger command of 1.0 will be accepted and trigger the operation. * The command_available function can be used to check if a command is available, * undefined commands (other than 1.0) will be ignored. * The set_response function should be used to set the response value. * It will then clear the command. * */ struct TriggerCommand { double command = std::numeric_limits::quiet_NaN(); double response = std::numeric_limits::quiet_NaN(); std::function trigger_function; bool try_trigger() { if (command == 1.0) { response = trigger_function() ? 1.0 : 0.0; command = std::numeric_limits::quiet_NaN(); return true; } command = std::numeric_limits::quiet_NaN(); return false; } }; } // namespace canopen_ros2_control #endif ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/robot_system.hpp ================================================ // Copyright (c) 2023, Fraunhofer IPA // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROL__ROBOT_SYSTEM_HPP_ #define CANOPEN_ROS2_CONTROL__ROBOT_SYSTEM_HPP_ #include "canopen_402_driver/cia402_driver.hpp" #include "canopen_core/configuration_manager.hpp" #include "canopen_core/device_container.hpp" #include "canopen_ros2_control/cia402_data.hpp" #include "canopen_ros2_control/visibility_control.h" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" namespace canopen_ros2_control { class RobotSystem : public hardware_interface::SystemInterface { public: CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC RobotSystem() : hardware_interface::SystemInterface() {} CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC ~RobotSystem() = default; /** * @brief Initialize the hardware interface * * Fetch the hardware info stored in the urdf. * Specifically these values: * - bus_config * - master_config * - can_interface_name * - master_bin * * @param info * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; /** * @brief Configure the hardware interface * * Create the device container and the executor. * Start the threads for running the canopen stack. * * @param info * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; /** * @brief Export the state interfaces for this system. * * The state interface of each cia402 device contains the following: * - Position * - Velocity * * @return std::vector */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_state_interfaces() override; /** * @brief Export the command interfaces for this system. * * The command interface of each cia402 device contains the following: * - Position * - Velocity * - Effort * - Operation Mode * - Init Command * - Halt Command * - Recover Command * * @return std::vector */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_command_interfaces() override; /** * @brief Cleanup the hardware interface * * @param previous_state * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; /** * @brief Shutdown the hardware interface * * @param previous_state * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; /** * @brief Activate the hardware interface * * @param previous_state * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; /** * @brief Deactivate the hardware interface * * @param previous_state * @return hardware_interface::CallbackReturn */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; /** * @brief Read the state from the hardware interface * * @return hardware_interface::return_type */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; /** * @brief Write the command to the hardware interface * * @return hardware_interface::return_type */ CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::return_type perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) override; protected: std::shared_ptr device_container_; std::shared_ptr executor_; std::vector robot_motor_data_; std::string bus_config_; std::string master_config_; std::string master_bin_; std::string can_interface_; std::unique_ptr spin_thread_; std::unique_ptr init_thread_; rclcpp::Logger robot_system_logger = rclcpp::get_logger("robot_system"); private: /** * @brief Spins the ROS executor * */ void spin(); void clean(); /** * @brief Initialize the device container * */ void initDeviceContainer(); }; } // namespace canopen_ros2_control #endif ================================================ FILE: canopen_ros2_control/include/canopen_ros2_control/visibility_control.h ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ #define CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) #define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) #else #define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) #define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_ROS2_CONTROL__VISIBILITY_BUILDING_DLL #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT #else #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT #endif #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC #define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL #else #define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) #define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT #if __GNUC__ >= 4 #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC #define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL #endif #define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE #endif #endif // CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_ros2_control/package.xml ================================================ canopen_ros2_control 0.3.2 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl Apache-2.0 ament_cmake canopen_402_driver canopen_core canopen_proxy_driver hardware_interface pluginlib rclcpp rclcpp_components rclcpp_lifecycle ros2_control_test_assets ament_cmake ================================================ FILE: canopen_ros2_control/src/canopen_system.cpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. //---------------------------------------------------------------------- /*!\file * * \author Lovro Ivanov lovro.ivanov@gmail.com * \date 2022-06-29 * */ //---------------------------------------------------------------------- #include "canopen_ros2_control/canopen_system.hpp" #include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" namespace { auto const kLogger = rclcpp::get_logger("CanopenSystem"); } namespace canopen_ros2_control { CanopenSystem::CanopenSystem() {} void CanopenSystem::clean() { executor_->cancel(); printf("Joining..."); spin_thread_->join(); printf("Joined!"); device_container_.reset(); executor_.reset(); if (init_thread_->joinable()) { init_thread_->join(); } init_thread_.reset(); executor_.reset(); spin_thread_.reset(); } CanopenSystem::~CanopenSystem() { clean(); } hardware_interface::CallbackReturn CanopenSystem::on_init( const hardware_interface::HardwareComponentInterfaceParams & params) { if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } RCLCPP_INFO(kLogger, "bus_config: '%s'", info_.hardware_parameters["bus_config"].c_str()); RCLCPP_INFO(kLogger, "master_config: '%s'", info_.hardware_parameters["master_config"].c_str()); RCLCPP_INFO( kLogger, "can_interface_name: '%s'", info_.hardware_parameters["can_interface_name"].c_str()); RCLCPP_INFO(kLogger, "master_bin: '%s'", info_.hardware_parameters["master_bin"].c_str()); return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CanopenSystem::on_configure( const rclcpp_lifecycle::State & previous_state) { executor_ = std::make_shared(); device_container_ = std::make_shared(executor_); executor_->add_node(device_container_); // threads spin_thread_ = std::make_unique(&CanopenSystem::spin, this); init_thread_ = std::make_unique(&CanopenSystem::initDeviceContainer, this); // actually wait for init phase to end if (init_thread_->joinable()) { init_thread_->join(); } else { RCLCPP_ERROR(kLogger, "Could not join init thread!"); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CanopenSystem::on_cleanup( const rclcpp_lifecycle::State & previous_state) { clean(); return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CanopenSystem::on_shutdown( const rclcpp_lifecycle::State & previous_state) { clean(); return CallbackReturn::SUCCESS; } void CanopenSystem::spin() { executor_->spin(); executor_->remove_node(device_container_); RCLCPP_INFO(kLogger, "Exiting spin thread..."); } void CanopenSystem::initDeviceContainer() { std::string tmp_master_bin = (info_.hardware_parameters["master_bin"] == "\"\"") ? "" : info_.hardware_parameters["master_bin"]; device_container_->init( info_.hardware_parameters["can_interface_name"], info_.hardware_parameters["master_config"], info_.hardware_parameters["bus_config"], tmp_master_bin); RCLCPP_INFO(kLogger, "Number of registered drivers: '%zu'", device_container_->count_drivers()); for (const auto & [node_id, driver] : device_container_->get_registered_drivers()) { auto proxy_driver = std::static_pointer_cast(driver); // initialize canopen data it not existing if (canopen_data_.find(node_id) == canopen_data_.end()) { canopen_data_[node_id] = CanopenNodeData(); } auto nmt_state_cb = [&](canopen::NmtState nmt_state, uint8_t id) { canopen_data_[id].nmt_state.set_state(nmt_state); }; // register callback proxy_driver->register_nmt_state_cb(nmt_state_cb); auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) { canopen_data_[id].set_rpdo_data(data); }; // register callback proxy_driver->register_rpdo_cb(rpdo_cb); auto emcy_cb = [&](ros2_canopen::COEmcy data, uint8_t id) { canopen_data_[id].emcy_data.set_emcy(data); }; // register callback proxy_driver->register_emcy_cb(emcy_cb); RCLCPP_INFO( kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '0x%X'", // driver->get_node_base_interface()->get_name(), node_id); } RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); } std::vector CanopenSystem::export_state_interfaces() { std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) { // skip adding canopen interfaces continue; } const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); // RCLCPP_INFO(kLogger, "node id on export state interface for joint: '%s' is '%s'", // info_.joints[i].name.c_str(), info_.joints[i].parameters["node_id"].c_str()); // rpdo index state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "rpdo/index", &canopen_data_[node_id].rpdo_data.index)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "rpdo/subindex", &canopen_data_[node_id].rpdo_data.subindex)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "rpdo/data", &canopen_data_[node_id].rpdo_data.data)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "nmt/state", &canopen_data_[node_id].nmt_state.state)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/error_code", &canopen_data_[node_id].emcy_data.error_code)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/error_register", &canopen_data_[node_id].emcy_data.error_register)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/manufacturer_error_code1", &canopen_data_[node_id].emcy_data.manufacturer_error_code1)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/manufacturer_error_code2", &canopen_data_[node_id].emcy_data.manufacturer_error_code2)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/manufacturer_error_code3", &canopen_data_[node_id].emcy_data.manufacturer_error_code3)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/manufacturer_error_code4", &canopen_data_[node_id].emcy_data.manufacturer_error_code4)); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "emcy/manufacturer_error_code5", &canopen_data_[node_id].emcy_data.manufacturer_error_code5)); } return state_interfaces; } std::vector CanopenSystem::export_command_interfaces() { std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) { // skip adding canopen interfaces continue; } const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "tpdo/index", &canopen_data_[node_id].tpdo_data.index)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "tpdo/subindex", &canopen_data_[node_id].tpdo_data.subindex)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "tpdo/data", &canopen_data_[node_id].tpdo_data.data)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "tpdo/ons", &canopen_data_[node_id].tpdo_data.one_shot)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "nmt/reset", &canopen_data_[node_id].nmt_state.reset_ons)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "nmt/reset_fbk", &canopen_data_[node_id].nmt_state.reset_fbk)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "nmt/start", &canopen_data_[node_id].nmt_state.start_ons)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "nmt/start_fbk", &canopen_data_[node_id].nmt_state.start_fbk)); } return command_interfaces; } hardware_interface::CallbackReturn CanopenSystem::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): prepare the robot to receive commands return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CanopenSystem::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): prepare the robot to stop receiving commands return CallbackReturn::SUCCESS; } hardware_interface::return_type CanopenSystem::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // TODO(anyone): read robot states // nmt state is set via Ros2ControlNmtState::set_state within nmt_state_cb // rpdo has a queue of messages, we read the latest one for (const auto & [node_id, canopen_data] : canopen_data_) { if (canopen_data.emcy_data.error_code != 0) { RCLCPP_ERROR( kLogger, "NodeID: 0x%X; Error code: %X; Error register: %X; Manufacturer error code: [%X, %X, %X, " "%X, %X];", node_id, canopen_data.emcy_data.original_emcy.eec, canopen_data.emcy_data.original_emcy.er, canopen_data.emcy_data.original_emcy.msef[0], canopen_data.emcy_data.original_emcy.msef[1], canopen_data.emcy_data.original_emcy.msef[2], canopen_data.emcy_data.original_emcy.msef[3], canopen_data.emcy_data.original_emcy.msef[4]); } } return hardware_interface::return_type::OK; } hardware_interface::return_type CanopenSystem::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // TODO(anyone): write robot's commands' auto drivers = device_container_->get_registered_drivers(); for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) { if (drivers.find(it->first) == drivers.end()) { // this is expected for NodeID 0x00 - why do we have it at all? RCLCPP_DEBUG(kLogger, "Driver for NodeID 0x%X not found. Skipping...", it->first); continue; } auto proxy_driver = std::static_pointer_cast(drivers[it->first]); // reset node nmt if (it->second.nmt_state.reset_command()) { it->second.nmt_state.reset_fbk = static_cast(proxy_driver->reset_node_nmt_command()); } // start nmt if (it->second.nmt_state.start_command()) { it->second.nmt_state.start_fbk = static_cast(proxy_driver->start_node_nmt_command()); } // tpdo data one shot mechanism if (it->second.tpdo_data.write_command()) { it->second.tpdo_data.prepare_data(); try { proxy_driver->tpdo_transmit(it->second.tpdo_data.original_data); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } } } return hardware_interface::return_type::OK; } } // namespace canopen_ros2_control #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::CanopenSystem, hardware_interface::SystemInterface) ================================================ FILE: canopen_ros2_control/src/cia402_system.cpp ================================================ // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. //---------------------------------------------------------------------- /*!\file * * \author Lovro Ivanov lovro.ivanov@gmail.com * \date 2022-08-01 * */ //---------------------------------------------------------------------- #include "canopen_ros2_control/cia402_system.hpp" #include namespace { auto const kLogger = rclcpp::get_logger("Cia402System"); } namespace canopen_ros2_control { Cia402System::Cia402System() : CanopenSystem() {} hardware_interface::CallbackReturn Cia402System::on_init( const hardware_interface::HardwareComponentInterfaceParams & params) { auto ret_val = CanopenSystem::on_init(params); if (ret_val != hardware_interface::CallbackReturn::SUCCESS) { return ret_val; } return CallbackReturn::SUCCESS; } void Cia402System::initDeviceContainer() { std::string tmp_master_bin = (info_.hardware_parameters["master_bin"] == "\"\"") ? "" : info_.hardware_parameters["master_bin"]; device_container_->init( info_.hardware_parameters["can_interface_name"], info_.hardware_parameters["master_config"], info_.hardware_parameters["bus_config"], tmp_master_bin); auto drivers = device_container_->get_registered_drivers(); RCLCPP_INFO(kLogger, "Number of registered drivers: '%lu'", device_container_->count_drivers()); for (auto it = drivers.begin(); it != drivers.end(); it++) { auto driver = std::static_pointer_cast(it->second); auto nmt_state_cb = [&](canopen::NmtState nmt_state, uint8_t id) { canopen_data_[id].nmt_state.set_state(nmt_state); }; // register callback driver->register_nmt_state_cb(nmt_state_cb); auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) { canopen_data_[id].rpdo_data.set_data(data); }; // register callback driver->register_rpdo_cb(rpdo_cb); RCLCPP_INFO( kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '0x%X'", it->second->get_node_base_interface()->get_name(), it->first); } RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); } hardware_interface::CallbackReturn Cia402System::on_configure( const rclcpp_lifecycle::State & previous_state) { executor_ = std::make_shared(); device_container_ = std::make_shared(executor_); executor_->add_node(device_container_); // threads spin_thread_ = std::make_unique(&Cia402System::spin, this); init_thread_ = std::make_unique(&Cia402System::initDeviceContainer, this); // actually wait for init phase to end if (init_thread_->joinable()) { init_thread_->join(); // TODO(livanov93): see how to handle configure once LifecycleCia402Driver is introduced /* auto drivers = device_container_->get_registered_drivers(); for (auto it = drivers.begin(); it != drivers.end(); it++) { auto d = std::static_pointer_cast(it->second); d->configure(); } */ } else { RCLCPP_ERROR(kLogger, "Could not join init thread!"); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } std::vector Cia402System::export_state_interfaces() { std::vector state_interfaces; // underlying base class export first state_interfaces = CanopenSystem::export_state_interfaces(); for (uint i = 0; i < info_.joints.size(); i++) { if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) { // skip adding motor canopen interfaces continue; } const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); // Get channel parameter (defaults to 0 for backward compatibility) uint8_t channel = 0; if (info_.joints[i].parameters.find("channel") != info_.joints[i].parameters.end()) { channel = static_cast(std::stoi(info_.joints[i].parameters["channel"])); } auto motor_key = getMotorKey(node_id, channel); // actual position state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &motor_data_[motor_key].actual_position)); // actual speed state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &motor_data_[motor_key].actual_speed)); // actual effort state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &motor_data_[motor_key].actual_effort)); } return state_interfaces; } std::vector Cia402System::export_command_interfaces() { std::vector command_interfaces; // underlying base class export first command_interfaces = CanopenSystem::export_command_interfaces(); for (uint i = 0; i < info_.joints.size(); i++) { if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) { // skip adding canopen interfaces continue; } const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); // Get channel parameter (defaults to 0 for backward compatibility) uint8_t channel = 0; if (info_.joints[i].parameters.find("channel") != info_.joints[i].parameters.end()) { channel = static_cast(std::stoi(info_.joints[i].parameters["channel"])); } auto motor_key = getMotorKey(node_id, channel); // target command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &motor_data_[motor_key].target.position_value)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &motor_data_[motor_key].target.velocity_value)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &motor_data_[motor_key].target.torque_value)); // init command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "init_cmd", &motor_data_[motor_key].init.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "init_fbk", &motor_data_[motor_key].init.resp)); // halt command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "halt_cmd", &motor_data_[motor_key].halt.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "halt_fbk", &motor_data_[motor_key].halt.resp)); // recover command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "recover_cmd", &motor_data_[motor_key].recover.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "recover_fbk", &motor_data_[motor_key].recover.resp)); // set position mode command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "position_mode_cmd", &motor_data_[motor_key].position_mode.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "position_mode_fbk", &motor_data_[motor_key].position_mode.resp)); // set velocity mode command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "velocity_mode_cmd", &motor_data_[motor_key].velocity_mode.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "velocity_mode_fbk", &motor_data_[motor_key].velocity_mode.resp)); // set cyclic velocity mode command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "cyclic_velocity_mode_cmd", &motor_data_[motor_key].cyclic_velocity_mode.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "cyclic_velocity_mode_fbk", &motor_data_[motor_key].cyclic_velocity_mode.resp)); // set cyclic position mode command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "cyclic_position_mode_cmd", &motor_data_[motor_key].cyclic_position_mode.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "cyclic_position_mode_fbk", &motor_data_[motor_key].cyclic_position_mode.resp)); // set interpolated position mode command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "interpolated_position_mode_cmd", &motor_data_[motor_key].interpolated_position_mode.ons_cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "interpolated_position_mode_fbk", &motor_data_[motor_key].interpolated_position_mode.resp)); } return command_interfaces; } hardware_interface::CallbackReturn Cia402System::on_activate( const rclcpp_lifecycle::State & previous_state) { return CanopenSystem::on_activate(previous_state); } hardware_interface::CallbackReturn Cia402System::on_deactivate( const rclcpp_lifecycle::State & previous_state) { return CanopenSystem::on_deactivate(previous_state); } hardware_interface::return_type Cia402System::read( const rclcpp::Time & time, const rclcpp::Duration & period) { // TODO(anyone): read robot states auto ret_val = CanopenSystem::read(time, period); auto drivers = device_container_->get_registered_drivers(); // Iterate through all motor data entries which now include channel info for (auto it = motor_data_.begin(); it != motor_data_.end(); ++it) { const auto & node_id = it->first.first; const auto & channel = it->first.second; auto motion_controller_driver = std::static_pointer_cast(drivers[node_id]); // get position motor_data_[it->first].actual_position = motion_controller_driver->get_position(channel); // get speed motor_data_[it->first].actual_speed = motion_controller_driver->get_speed(channel); // get effort motor_data_[it->first].actual_effort = motion_controller_driver->get_effort(channel); } return ret_val; } hardware_interface::return_type Cia402System::write( const rclcpp::Time & time, const rclcpp::Duration & period) { auto drivers = device_container_->get_registered_drivers(); // Process NMT commands per node (not per channel) for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) { auto motion_controller_driver = std::static_pointer_cast(drivers[it->first]); // do same as in proxy system first - handle nmt, tpdo, rpdo // reset node nmt if (it->second.nmt_state.reset_command()) { motion_controller_driver->reset_node_nmt_command(); } // start nmt if (it->second.nmt_state.start_command()) { motion_controller_driver->start_node_nmt_command(); } // tpdo data one shot mechanism if (it->second.tpdo_data.write_command()) { it->second.tpdo_data.prepare_data(); motion_controller_driver->tpdo_transmit(it->second.tpdo_data.original_data); } } // Process motor commands per channel for (auto it = motor_data_.begin(); it != motor_data_.end(); ++it) { const auto & node_id = it->first.first; const auto & channel = it->first.second; auto motion_controller_driver = std::static_pointer_cast(drivers[node_id]); // initialisation handleInit(node_id, channel, motion_controller_driver); // halt handleHalt(node_id, channel, motion_controller_driver); // recover handleRecover(node_id, channel, motion_controller_driver); // mode switching switchModes(node_id, channel, motion_controller_driver); const uint16_t & mode = motion_controller_driver->get_mode(channel); switch (mode) { case MotorBase::No_Mode: break; case MotorBase::Profiled_Position: case MotorBase::Cyclic_Synchronous_Position: case MotorBase::Interpolated_Position: motion_controller_driver->set_target(motor_data_[it->first].target.position_value, channel); break; case MotorBase::Profiled_Velocity: case MotorBase::Cyclic_Synchronous_Velocity: motion_controller_driver->set_target(motor_data_[it->first].target.velocity_value, channel); break; case MotorBase::Profiled_Torque: motion_controller_driver->set_target(motor_data_[it->first].target.torque_value, channel); break; default: RCLCPP_INFO(kLogger, "Mode %u not supported", mode); } } return hardware_interface::return_type::OK; } void Cia402System::switchModes( uint id, uint channel, const std::shared_ptr & driver) { auto motor_key = getMotorKey(id, channel); if (motor_data_[motor_key].position_mode.is_commanded()) { motor_data_[motor_key].position_mode.set_response( driver->set_operation_mode(MotorBase::Profiled_Position, channel)); } if (motor_data_[motor_key].cyclic_position_mode.is_commanded()) { motor_data_[motor_key].cyclic_position_mode.set_response( driver->set_operation_mode(MotorBase::Cyclic_Synchronous_Position, channel)); } if (motor_data_[motor_key].velocity_mode.is_commanded()) { motor_data_[motor_key].velocity_mode.set_response( driver->set_operation_mode(MotorBase::Profiled_Velocity, channel)); } if (motor_data_[motor_key].cyclic_velocity_mode.is_commanded()) { motor_data_[motor_key].cyclic_velocity_mode.set_response( driver->set_operation_mode(MotorBase::Cyclic_Synchronous_Velocity, channel)); } if (motor_data_[motor_key].torque_mode.is_commanded()) { motor_data_[motor_key].torque_mode.set_response( driver->set_operation_mode(MotorBase::Profiled_Torque, channel)); } if (motor_data_[motor_key].interpolated_position_mode.is_commanded()) { motor_data_[motor_key].interpolated_position_mode.set_response( driver->set_operation_mode(MotorBase::Interpolated_Position, channel)); } } void Cia402System::handleInit( uint id, uint channel, const std::shared_ptr & driver) { auto motor_key = getMotorKey(id, channel); if (motor_data_[motor_key].init.is_commanded()) { motor_data_[motor_key].init.set_response(driver->init_motor(channel)); } } void Cia402System::handleRecover( uint id, uint channel, const std::shared_ptr & driver) { auto motor_key = getMotorKey(id, channel); if (motor_data_[motor_key].recover.is_commanded()) { motor_data_[motor_key].recover.set_response(driver->recover_motor(channel)); } } void Cia402System::handleHalt( uint id, uint channel, const std::shared_ptr & driver) { auto motor_key = getMotorKey(id, channel); if (motor_data_[motor_key].halt.is_commanded()) { motor_data_[motor_key].halt.set_response(driver->halt_motor(channel)); } } } // namespace canopen_ros2_control #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::Cia402System, hardware_interface::SystemInterface) ================================================ FILE: canopen_ros2_control/src/robot_system.cpp ================================================ // Copyright (c) 2023, Fraunhofer IPA // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_ros2_control/robot_system.hpp" #include using namespace canopen_ros2_control; // auto robot_system_logger = rclcpp::get_logger("robot_system_interface"); hardware_interface::CallbackReturn RobotSystem::on_init( const hardware_interface::HardwareComponentInterfaceParams & params) { if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } robot_system_logger = rclcpp::get_logger(info_.name + "_interface"); RCLCPP_INFO(robot_system_logger, "Registering hardware interface '%s'", info_.name.c_str()); // Check bus config is specified. if (info_.hardware_parameters.find("bus_config") == info_.hardware_parameters.end()) { RCLCPP_ERROR( robot_system_logger, "No bus_config parameter provided for '%s' hardware interface.", info_.name.c_str()); return CallbackReturn::ERROR; } bus_config_ = info_.hardware_parameters["bus_config"]; RCLCPP_INFO( robot_system_logger, "'%s' has bus config: '%s'", info_.name.c_str(), bus_config_.c_str()); // Check master config is specified. if (info_.hardware_parameters.find("master_config") == info_.hardware_parameters.end()) { RCLCPP_ERROR( robot_system_logger, "No master_config parameter provided for '%s' hardware interface.", info_.name.c_str()); return CallbackReturn::ERROR; } master_config_ = info_.hardware_parameters["master_config"]; RCLCPP_INFO( robot_system_logger, "'%s' has master config: '%s'", info_.name.c_str(), master_config_.c_str()); // Check master bin is specified. if (info_.hardware_parameters.find("master_bin") != info_.hardware_parameters.end()) { master_bin_ = info_.hardware_parameters["master_bin"]; if (master_bin_ == "\"\"") { master_bin_ = ""; } RCLCPP_INFO( robot_system_logger, "'%s' has master bin: '%s'", info_.name.c_str(), master_bin_.c_str()); } else { master_bin_ = ""; } // Check can_interface_name is specified. if (info_.hardware_parameters.find("can_interface_name") == info_.hardware_parameters.end()) { RCLCPP_ERROR( robot_system_logger, "No can_interface_name parameter provided for '%s' hardware interface.", info_.name.c_str()); return CallbackReturn::ERROR; } can_interface_ = info_.hardware_parameters["can_interface_name"]; RCLCPP_INFO( robot_system_logger, "'%s' has can interface: '%s'", info_.name.c_str(), can_interface_.c_str()); ros2_canopen::ConfigurationManager config(bus_config_); config.init_config(); // Load joint data for (auto joint : info_.joints) { auto driver_type = config.get_entry(joint.parameters["device_name"], "driver").value(); if (driver_type == "ros2_canopen::Cia402Driver") { auto data = Cia402Data(); if (data.init_data(joint, config.dump_device(joint.parameters["device_name"]))) { robot_motor_data_.push_back(data); } } else { RCLCPP_ERROR( robot_system_logger, "Driver type '%s' not supported for joint '%s'", driver_type.c_str(), joint.name.c_str()); return CallbackReturn::ERROR; } } return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RobotSystem::on_configure( const rclcpp_lifecycle::State & previous_state) { executor_ = std::make_shared(rclcpp::ExecutorOptions(), 2); device_container_ = std::make_shared(executor_); executor_->add_node(device_container_); spin_thread_ = std::make_unique(&RobotSystem::spin, this); init_thread_ = std::make_unique(&RobotSystem::initDeviceContainer, this); if (init_thread_->joinable()) { init_thread_->join(); } else { RCLCPP_ERROR(robot_system_logger, "Could not join init thread!"); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RobotSystem::on_activate( const rclcpp_lifecycle::State & previous_state) { for (auto & data : robot_motor_data_) { if (!data.driver->init_motor()) { RCLCPP_ERROR(robot_system_logger, "Failed to activate '%s'", data.joint_name.c_str()); return CallbackReturn::FAILURE; } } return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RobotSystem::on_deactivate( const rclcpp_lifecycle::State & previous_state) { for (auto & data : robot_motor_data_) { if (!data.driver->halt_motor()) { RCLCPP_ERROR(robot_system_logger, "Failed to deactivate '%s'", data.joint_name.c_str()); return CallbackReturn::FAILURE; } } return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RobotSystem::on_cleanup( const rclcpp_lifecycle::State & previous_state) { clean(); return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RobotSystem::on_shutdown( const rclcpp_lifecycle::State & previous_state) { clean(); return CallbackReturn::SUCCESS; } std::vector RobotSystem::export_state_interfaces() { std::vector state_interfaces; // Iterate over joints in xacro for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) { data.export_state_interface(state_interfaces); } return state_interfaces; } std::vector RobotSystem::export_command_interfaces() { std::vector command_interfaces; // Iterate over joints in xacro for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) { data.export_command_interface(command_interfaces); } return command_interfaces; } hardware_interface::return_type RobotSystem::read( const rclcpp::Time & time, const rclcpp::Duration & period) { // Iterate over joints for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) { data.read_state(); } return hardware_interface::return_type::OK; } hardware_interface::return_type RobotSystem::write( const rclcpp::Time & time, const rclcpp::Duration & period) { for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) { data.write_target(); } return hardware_interface::return_type::OK; } hardware_interface::return_type RobotSystem::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { // register interfaces to start per device for (auto interface : start_interfaces) { auto it = std::find_if( robot_motor_data_.begin(), robot_motor_data_.end(), [interface](Cia402Data & data) { return std::find(data.interfaces.begin(), data.interfaces.end(), interface) != data.interfaces.end(); }); if (it != robot_motor_data_.end()) { it->interfaces_to_start.push_back( hardware_interface::CommandInterface(interface).get_interface_name()); } } // register interfaces to stop per device for (auto interface : stop_interfaces) { auto it = std::find_if( robot_motor_data_.begin(), robot_motor_data_.end(), [interface](Cia402Data & data) { return std::find(data.interfaces.begin(), data.interfaces.end(), interface) != data.interfaces.end(); }); if (it != robot_motor_data_.end()) { it->interfaces_to_stop.push_back( hardware_interface::CommandInterface(interface).get_interface_name()); } } // perform switching for (auto & data : robot_motor_data_) { if (!data.perform_switch()) { return hardware_interface::return_type::ERROR; } } return hardware_interface::return_type::OK; } void RobotSystem::initDeviceContainer() { // Init device container device_container_->init( this->can_interface_, this->master_config_, this->bus_config_, this->master_bin_); // Get all registered drivers. auto drivers = device_container_->get_registered_drivers(); // Iterate over all drivers and allocate them to the correct joint. for (auto & data : robot_motor_data_) { // Find correct driver for joint via node id. auto driver = std::find_if( drivers.begin(), drivers.end(), [&data](const std::pair> & driver) { return driver.first == data.node_id; }); if (driver == drivers.end()) { RCLCPP_ERROR( device_container_->get_logger(), "Could not find driver for joint '%s' with node id '%d'", data.joint_name.c_str(), data.node_id); continue; } // Allocate driver to joint. if ( device_container_->get_driver_type(driver->first).compare("ros2_canopen::Cia402Driver") == 0) { data.driver = std::static_pointer_cast(driver->second); } } RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); } void RobotSystem::spin() { executor_->spin(); executor_->remove_node(device_container_); RCLCPP_INFO(device_container_->get_logger(), "Stopped spinning RobotSystem ROS2 executor"); } void RobotSystem::clean() { printf("Cancel exectutor..."); executor_->cancel(); printf("Join spin thread..."); spin_thread_->join(); printf("Reset variables..."); device_container_.reset(); executor_.reset(); init_thread_->join(); init_thread_.reset(); executor_.reset(); spin_thread_.reset(); robot_motor_data_.clear(); } #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::RobotSystem, hardware_interface::SystemInterface) ================================================ FILE: canopen_ros2_control/test/test_canopen_system.cpp ================================================ // Copyright (c) 2022, StoglRobotics // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. //---------------------------------------------------------------------- /*!\file * * \author Lovro Ivanov lovro.ivanov@gmail.com * \date 2022-06-29 * */ //---------------------------------------------------------------------- #include #include #include "hardware_interface/resource_manager.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" class TestCanopenSystem : public ::testing::Test { protected: void SetUp() override { // TODO(anyone): Extend this description to your robot canopen_system_2dof_ = R"( canopen_ros2_control/CanopenSystem 1.57 0.7854 )"; } std::string canopen_system_2dof_; }; TEST_F(TestCanopenSystem, load_canopen_system_2dof) { auto urdf = ros2_control_test_assets::urdf_head + canopen_system_2dof_ + ros2_control_test_assets::urdf_tail; ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } ================================================ FILE: canopen_ros2_controllers/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * Merge pull request `#400 `_ from ros-industrial/fix/proxy_controller Update controllers for ROS Rolling API changes and fix fake slave type handling * Enhance command interface error handling and feedback reporting in CanopenProxyController * Contributors: Vishnuprasad Prachandabhanu 0.2.9 (2024-04-16) ------------------ 0.3.1 (2025-06-23) ------------------ * Add boot timeout and retry * Fix command interfaces value missmatch. * Fix realtime-tools include header file * Include upstream changes from 'master'. * Contributors: Gerry Salinas, Marco A. Gutierrez, Vishnuprasad Prachandabhanu 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Contributors: ipa-vsp 0.2.8 (2024-01-19) ------------------ * Remove controller tests (`#249 `_) * Disable controller tests for now (`#231 `_) * Disable controller tests for now * Update iron.yml * Fix ci * Contributors: Christoph Hellmann Santos, Vishnuprasad Prachandabhanu 0.2.7 (2023-06-30) ------------------ * Correct Proxy controller after changes and update tests. * Contributors: Dr. Denis, Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Fix QoS build warning canopen_ros2_controllers * Don't use ros2_control_test_assets since not needed anymore. Add load tests for all controllers. * Contributors: Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Dr.-Ing. Denis Štogl, Lovro, livanov93 ================================================ FILE: canopen_ros2_controllers/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_ros2_controllers) find_package(ament_cmake REQUIRED) find_package(canopen_402_driver REQUIRED) find_package(canopen_interfaces REQUIRED) find_package(canopen_proxy_driver REQUIRED) find_package(controller_interface REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) add_library( ${PROJECT_NAME} SHARED src/canopen_proxy_controller.cpp src/cia402_device_controller.cpp ) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) target_include_directories(${PROJECT_NAME} PUBLIC $ $ ) target_link_libraries(${PROJECT_NAME} PUBLIC ${canopen_interfaces_TARGETS} ${std_msgs_TARGETS} ${std_srvs_TARGETS} canopen_402_driver::cia402_driver canopen_402_driver::lely_motion_controller_bridge canopen_402_driver::lifecycle_cia402_driver canopen_402_driver::node_canopen_cia402_driver canopen_proxy_driver::lifecycle_proxy_driver canopen_proxy_driver::node_canopen_proxy_driver canopen_proxy_driver::proxy_driver controller_interface::controller_interface controller_manager::controller_manager controller_manager::controller_manager_parameters hardware_interface::hardware_interface hardware_interface::mock_components pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle realtime_tools::realtime_tools realtime_tools::thread_priority ) target_compile_definitions(${PROJECT_NAME} PRIVATE "CANOPEN_PROXY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface canopen_ros2_controllers.xml) install( TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib INCLUDES DESTINATION include ) install( DIRECTORY include/ DESTINATION include ) if(FALSE) # find_package(ament_cmake_gmock REQUIRED) # ament_add_gmock(test_load_canopen_proxy_controller test/test_load_canopen_proxy_controller.cpp) # target_include_directories(test_load_canopen_proxy_controller PRIVATE include) # target_link_libraries(test_load_canopen_proxy_controller ... # ament_add_gmock(test_load_cia402_device_controller test/test_load_cia402_device_controller.cpp) # target_include_directories(test_load_cia402_device_controller PRIVATE include) # target_link_libraries(test_load_cia402_device_controller ... # ament_add_gmock(test_load_cia402_robot_controller test/test_load_cia402_robot_controller.cpp) # target_include_directories(test_load_cia402_robot_controller PRIVATE include) # target_link_libraries(test_load_cia402_robot_controller ... # ament_add_gmock(test_canopen_proxy_controller test/test_canopen_proxy_controller.cpp) # target_include_directories(test_canopen_proxy_controller PRIVATE include) # target_link_libraries(test_canopen_proxy_controller ... endif() ament_export_include_directories( include ) ament_export_libraries( ${PROJECT_NAME} ) ament_export_dependencies( canopen_402_driver canopen_interfaces canopen_proxy_driver controller_interface controller_manager hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs std_srvs ) ament_package() ================================================ FILE: canopen_ros2_controllers/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_ros2_controllers/canopen_ros2_controllers.xml ================================================ Generic controller for Canopen devices providing service interfaces through ros2_control stack to the can devices. Generic controller for Canopen 402 devices providing service interfaces through ros2_control stack to the can devices. Generic controller for Canopen 402 devices providing service interfaces through ros2_control stack to the can devices. ================================================ FILE: canopen_ros2_controllers/include/canopen_ros2_controllers/canopen_proxy_controller.hpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ #define CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ #include #include #include #include "canopen_interfaces/msg/co_data.hpp" #include "canopen_interfaces/srv/co_read.hpp" #include "canopen_interfaces/srv/co_write.hpp" #include "canopen_ros2_controllers/visibility_control.h" #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" namespace { enum CommandInterfaces { TPDO_INDEX, TPDO_SUBINDEX, TPDO_DATA, TPDO_ONS, NMT_RESET, NMT_RESET_FBK, NMT_START, NMT_START_FBK, LAST_COMMAND_AUX, }; enum StateInterfaces { RPDO_INDEX, RPDO_SUBINDEX, RPDO_DATA, NMT_STATE, LAST_STATE_AUX, }; } // namespace namespace canopen_ros2_controllers { class CanopenProxyController : public controller_interface::ControllerInterface { public: CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC CanopenProxyController(); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; using ControllerCommandMsg = canopen_interfaces::msg::COData; using ControllerStartResetSrvType = std_srvs::srv::Trigger; using ControllerSDOReadSrvType = canopen_interfaces::srv::CORead; using ControllerSDOWriteSrvType = canopen_interfaces::srv::COWrite; using ControllerNMTStateMsg = std_msgs::msg::String; protected: std::string joint_name_; // Command subscribers // TPDO subscription rclcpp::Subscription::SharedPtr tpdo_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_cmd_; // NMT state publisher using ControllerNmtStateRTPublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr nmt_state_pub_; std::unique_ptr nmt_state_rt_publisher_; std::string nmt_state_actual_ = "BOOTUP"; // RPDO publisher using ControllerRPDOPRTublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr rpdo_pub_; std::unique_ptr rpdo_rt_publisher_; // NMT reset service rclcpp::Service::SharedPtr nmt_state_reset_service_; // NMT start service rclcpp::Service::SharedPtr nmt_state_start_service_; // SDO read service rclcpp::Service::SharedPtr sdo_read_service_; // SDO write service rclcpp::Service::SharedPtr sdo_write_service_; }; } // namespace canopen_ros2_controllers #endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ ================================================ FILE: canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ #define CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ #include "canopen_interfaces/srv/co_target_double.hpp" #include "canopen_ros2_controllers/canopen_proxy_controller.hpp" static constexpr int kLoopPeriodMS = 100; static constexpr double kCommandValue = 1.0; namespace { enum Cia402CommandInterfaces { INIT_CMD = CommandInterfaces::LAST_COMMAND_AUX, INIT_FBK, HALT_CMD, HALT_FBK, RECOVER_CMD, RECOVER_FBK, POSITION_MODE_CMD, POSITION_MODE_FBK, VELOCITY_MODE_CMD, VELOCITY_MODE_FBK, CYCLIC_VELOCITY_MODE_CMD, CYCLIC_VELOCITY_MODE_FBK, CYCLIC_POSITION_MODE_CMD, CYCLIC_POSITION_MODE_FBK, INTERPOLATED_POSITION_MODE_CMD, INTERPOLATED_POSITION_MODE_FBK, }; enum Cia402StateInterfaces { FIRST_STATE = StateInterfaces::LAST_STATE_AUX, }; } // namespace namespace canopen_ros2_controllers { class Cia402DeviceController : public canopen_ros2_controllers::CanopenProxyController { public: CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC Cia402DeviceController(); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init(); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const; CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state); CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period); protected: inline rclcpp::Service::SharedPtr createTriggerSrv( const std::string & service, Cia402CommandInterfaces cmd, Cia402CommandInterfaces fbk) { // define service profile Qos auto service_profile = rclcpp::QoS(1); service_profile.keep_all(); rclcpp::Service::SharedPtr srv = get_node()->create_service( service, [&, cmd, fbk]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { response->success = false; const bool sent = command_interfaces_[cmd].set_value(kCommandValue); if (!sent) { RCLCPP_WARN( this->get_node()->get_logger(), "Failed to send command interface: %d request", cmd); } while (rclcpp::ok()) { const auto fbk_value = command_interfaces_[fbk].get_optional(); if (fbk_value && !std::isnan(*fbk_value)) { response->success = static_cast(*fbk_value); break; } std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); } // reset to nan if (!command_interfaces_[fbk].set_value(std::numeric_limits::quiet_NaN())) { RCLCPP_WARN( this->get_node()->get_logger(), "Failed to reset feedback for service %d", fbk); } if (!command_interfaces_[cmd].set_value(std::numeric_limits::quiet_NaN())) { RCLCPP_WARN( this->get_node()->get_logger(), "Failed to reset command for service %d", cmd); } }, service_profile); return srv; } rclcpp::Service::SharedPtr handle_init_service_; rclcpp::Service::SharedPtr handle_halt_service_; rclcpp::Service::SharedPtr handle_recover_service_; rclcpp::Service::SharedPtr handle_set_mode_position_service_; rclcpp::Service::SharedPtr handle_set_mode_torque_service_; rclcpp::Service::SharedPtr handle_set_mode_velocity_service_; rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service_; rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service_; rclcpp::Service::SharedPtr handle_set_mode_interpolated_position_service_; rclcpp::Service::SharedPtr handle_set_target_service_; }; } // namespace canopen_ros2_controllers #endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ ================================================ FILE: canopen_ros2_controllers/include/canopen_ros2_controllers/visibility_control.h ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) #else #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) #endif #ifdef CANOPEN_ROS2_CONTROLLERS__VISIBILITY_BUILDING_DLL #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT #else #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT #endif #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC_TYPE CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL #else #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT #if __GNUC__ >= 4 #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL #endif #define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC_TYPE #endif #endif // CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ ================================================ FILE: canopen_ros2_controllers/package.xml ================================================ canopen_ros2_controllers 0.3.2 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov Apache-2.0 ament_cmake canopen_402_driver canopen_interfaces canopen_proxy_driver controller_interface controller_manager hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs std_srvs ament_cmake ================================================ FILE: canopen_ros2_controllers/src/canopen_proxy_controller.cpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_ros2_controllers/canopen_proxy_controller.hpp" #include #include #include #include #include #include #include #include "controller_interface/helpers.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" static constexpr int kLoopPeriodMS = 100; static constexpr double kCommandValue = 1.0; namespace { // utility using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; // called from RT control loop bool propagate_controller_command_msg(std::shared_ptr & msg) { // TODO (livanov93): add better logic to decide if a message // should be propagated to the bus // check if it is 8 = uint8_t or 16 = uint16_t or 32 = uint32_t return true; } } // namespace namespace canopen_ros2_controllers { CanopenProxyController::CanopenProxyController() : controller_interface::ControllerInterface() {} controller_interface::CallbackReturn CanopenProxyController::on_init() { try { // use auto declare auto_declare("joint", joint_name_); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn CanopenProxyController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { if (parameter.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); return true; } return false; }; auto get_string_array_param_and_error_if_empty = [&](std::vector & parameter, const char * parameter_name) { parameter = get_node()->get_parameter(parameter_name).as_string_array(); return error_if_empty(parameter, parameter_name); }; auto get_string_param_and_error_if_empty = [&](std::string & parameter, const char * parameter_name) { parameter = get_node()->get_parameter(parameter_name).as_string(); return error_if_empty(parameter, parameter_name); }; if (get_string_param_and_error_if_empty(joint_name_, "joint")) { return controller_interface::CallbackReturn::ERROR; } // Command Subscriber and callbacks auto callback_cmd = [&](const std::shared_ptr msg) -> void { input_cmd_.writeFromNonRT(msg); }; tpdo_subscriber_ = get_node()->create_subscription( "~/tpdo", rclcpp::SystemDefaultsQoS(), callback_cmd); input_cmd_.writeFromNonRT(nullptr); try { // nmt state publisher nmt_state_pub_ = get_node()->create_publisher( "~/nmt_state", rclcpp::SystemDefaultsQoS()); nmt_state_rt_publisher_ = std::make_unique(nmt_state_pub_); // rpdo publisher rpdo_pub_ = get_node()->create_publisher("~/rpdo", rclcpp::SystemDefaultsQoS()); rpdo_rt_publisher_ = std::make_unique(rpdo_pub_); } catch (const std::exception & e) { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } if (nmt_state_rt_publisher_) { ControllerNMTStateMsg msg; msg.data = std::string(); bool published = nmt_state_rt_publisher_->try_publish(msg); if (!published) { RCLCPP_WARN( get_node()->get_logger(), "Could not publish initial NMT state message on controller '%s'", joint_name_.c_str()); } } if (rpdo_rt_publisher_) { ControllerCommandMsg msg; msg.index = 0u; msg.subindex = 0u; msg.data = 0u; bool published = rpdo_rt_publisher_->try_publish(msg); if (!published) { RCLCPP_WARN( get_node()->get_logger(), "Could not publish initial RPDO message on controller '%s'", joint_name_.c_str()); } } // init services // NMT reset auto on_nmt_state_reset = [&]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { response->success = false; const auto logger = get_node()->get_logger(); if (!command_interfaces_[CommandInterfaces::NMT_RESET].set_value(kCommandValue)) { RCLCPP_WARN(logger, "Failed to set NMT reset command"); } while (rclcpp::ok()) { const auto reset_cmd_value = command_interfaces_[CommandInterfaces::NMT_RESET].get_optional(); if (reset_cmd_value && std::isnan(*reset_cmd_value)) { break; } std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); } // report success const auto reset_feedback = command_interfaces_[CommandInterfaces::NMT_RESET_FBK].get_optional(); if (reset_feedback && !std::isnan(*reset_feedback)) { response->success = static_cast(*reset_feedback); } // reset to nan if (!command_interfaces_[CommandInterfaces::NMT_RESET_FBK].set_value( std::numeric_limits::quiet_NaN())) { RCLCPP_WARN(logger, "Failed to reset NMT reset feedback interface"); } }; auto service_profile = rclcpp::QoS(1); service_profile.keep_all(); nmt_state_reset_service_ = get_node()->create_service( "~/nmt_reset_node", on_nmt_state_reset, service_profile); // NMT start auto on_nmt_state_start = [&]( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { response->success = false; const auto logger = get_node()->get_logger(); if (!command_interfaces_[CommandInterfaces::NMT_START].set_value(kCommandValue)) { RCLCPP_WARN(logger, "Failed to send command for NMT start service"); } while (rclcpp::ok()) { const auto reset_fbk = command_interfaces_[CommandInterfaces::NMT_START_FBK].get_optional(); if (reset_fbk && !std::isnan(*reset_fbk)) { break; } std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); } // report success const auto start_feedback = command_interfaces_[CommandInterfaces::NMT_START_FBK].get_optional(); if (start_feedback && !std::isnan(*start_feedback)) { response->success = static_cast(*start_feedback); } // reset to nan if (!command_interfaces_[CommandInterfaces::NMT_START_FBK].set_value( std::numeric_limits::quiet_NaN())) { RCLCPP_WARN(logger, "Failed to reset NMT start feedback interface"); } }; nmt_state_start_service_ = get_node()->create_service( "~/nmt_start_node", on_nmt_state_start, service_profile); // SDO read auto on_sdo_read = [&]( const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response) {}; sdo_read_service_ = get_node()->create_service( "~/sdo_read", on_sdo_read, service_profile); // SDO write auto on_sdo_write = [&]( const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response) { }; sdo_write_service_ = get_node()->create_service( "~/sdo_write", on_sdo_write, service_profile); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration CanopenProxyController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(8); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/index"); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/subindex"); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/data"); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/ons"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/start"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/start_fbk"); return command_interfaces_config; } controller_interface::InterfaceConfiguration CanopenProxyController::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; state_interfaces_config.names.reserve(4); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/index"); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/subindex"); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/data"); state_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/state"); return state_interfaces_config; } controller_interface::CallbackReturn CanopenProxyController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command if (input_cmd_.readFromRT()) { *(input_cmd_.readFromRT()) = nullptr; } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn CanopenProxyController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // instead of a loop auto warn_on_failure = [&](bool ok, const char * name) { if (!ok) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 5000, "Failed to reset command interface %s during deactivate", name); } }; for (size_t i = 0; i < command_interfaces_.size(); ++i) { warn_on_failure( command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()), command_interfaces_[i].get_name().c_str()); } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type CanopenProxyController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { // nmt state is retrieved in SystemInterface via cb and is exposed here if (nmt_state_rt_publisher_) { auto message = std_msgs::msg::String(); const auto nmt_state_optional = state_interfaces_[StateInterfaces::NMT_STATE].get_optional(); if (nmt_state_optional) { auto nmt_state = static_cast(*nmt_state_optional); switch (static_cast(nmt_state)) { case canopen::NmtState::BOOTUP: message.data = "BOOTUP"; break; case canopen::NmtState::PREOP: message.data = "PREOP"; break; case canopen::NmtState::RESET_COMM: message.data = "RESET_COMM"; break; case canopen::NmtState::RESET_NODE: message.data = "RESET_NODE"; break; case canopen::NmtState::START: message.data = "START"; break; case canopen::NmtState::STOP: message.data = "STOP"; break; case canopen::NmtState::TOGGLE: message.data = "TOGGLE"; break; default: RCLCPP_ERROR(get_node()->get_logger(), "Unknown NMT State."); message.data = "ERROR"; break; } if (nmt_state_actual_ != message.data) { // publish on change only nmt_state_actual_ = std::string(message.data); const bool published = nmt_state_rt_publisher_->try_publish(message); if (!published) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 5000, "Failed to publish NMT state"); } } } } // exposing rpdo data via real-time publisher if (rpdo_rt_publisher_) { const auto rpdo_index = state_interfaces_[StateInterfaces::RPDO_INDEX].get_optional(); const auto rpdo_subindex = state_interfaces_[StateInterfaces::RPDO_SUBINDEX].get_optional(); const auto rpdo_data = state_interfaces_[StateInterfaces::RPDO_DATA].get_optional(); if (rpdo_index && rpdo_subindex && rpdo_data) { ControllerCommandMsg msg; msg.index = static_cast(*rpdo_index); msg.subindex = static_cast(*rpdo_subindex); msg.data = static_cast(*rpdo_data); const bool published = rpdo_rt_publisher_->try_publish(msg); if (!published) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 5000, "Failed to publish RPDO message"); } } } // tpdo data is the main controller data retrieved via subscription auto current_cmd = input_cmd_.readFromRT(); if (!current_cmd || !(*current_cmd)) { return controller_interface::return_type::OK; } else if (propagate_controller_command_msg(*current_cmd)) { auto warn_on_failure = [&](bool ok, const char * name) { if (!ok) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 5000, "Failed to set TPDO command %s", name); } }; warn_on_failure( command_interfaces_[CommandInterfaces::TPDO_INDEX].set_value( static_cast((*current_cmd)->index)), "index"); warn_on_failure( command_interfaces_[CommandInterfaces::TPDO_SUBINDEX].set_value( static_cast((*current_cmd)->subindex)), "subindex"); warn_on_failure( command_interfaces_[CommandInterfaces::TPDO_DATA].set_value( static_cast((*current_cmd)->data)), "data"); // tpdo data one shot mechanism warn_on_failure( command_interfaces_[CommandInterfaces::TPDO_ONS].set_value(kCommandValue), "ons"); *(input_cmd_.readFromRT()) = nullptr; } return controller_interface::return_type::OK; } } // namespace canopen_ros2_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( canopen_ros2_controllers::CanopenProxyController, controller_interface::ControllerInterface) ================================================ FILE: canopen_ros2_controllers/src/cia402_device_controller.cpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "canopen_ros2_controllers/cia402_device_controller.hpp" #include #include #include #include #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "controller_interface/helpers.hpp" namespace { // utility using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; // called from RT control loop void reset_controller_command_msg( std::shared_ptr & msg, const std::string & joint_name) { } } // namespace namespace canopen_ros2_controllers { Cia402DeviceController::Cia402DeviceController() : canopen_ros2_controllers::CanopenProxyController() { } controller_interface::CallbackReturn Cia402DeviceController::on_init() { if (CanopenProxyController::on_init() != controller_interface::CallbackReturn::SUCCESS) return controller_interface::CallbackReturn::ERROR; handle_init_service_ = createTriggerSrv( "~/init", Cia402CommandInterfaces::INIT_CMD, Cia402CommandInterfaces::INIT_FBK); handle_halt_service_ = createTriggerSrv( "~/halt", Cia402CommandInterfaces::HALT_CMD, Cia402CommandInterfaces::HALT_FBK); handle_recover_service_ = createTriggerSrv( "~/recover", Cia402CommandInterfaces::RECOVER_CMD, Cia402CommandInterfaces::RECOVER_FBK); handle_set_mode_position_service_ = createTriggerSrv( "~/position_mode", Cia402CommandInterfaces::POSITION_MODE_CMD, Cia402CommandInterfaces::POSITION_MODE_FBK); handle_set_mode_velocity_service_ = createTriggerSrv( "~/velocity_mode", Cia402CommandInterfaces::VELOCITY_MODE_CMD, Cia402CommandInterfaces::VELOCITY_MODE_FBK); handle_set_mode_cyclic_velocity_service_ = createTriggerSrv( "~/cyclic_velocity_mode", Cia402CommandInterfaces::CYCLIC_VELOCITY_MODE_CMD, Cia402CommandInterfaces::CYCLIC_VELOCITY_MODE_FBK); handle_set_mode_cyclic_position_service_ = createTriggerSrv( "~/cyclic_position_mode", Cia402CommandInterfaces::CYCLIC_POSITION_MODE_CMD, Cia402CommandInterfaces::CYCLIC_POSITION_MODE_FBK); handle_set_mode_interpolated_position_service_ = createTriggerSrv( "~/interpolated_position_mode", Cia402CommandInterfaces::INTERPOLATED_POSITION_MODE_CMD, Cia402CommandInterfaces::INTERPOLATED_POSITION_MODE_FBK); /* handle_set_mode_torque_service_ = createTriggerSrv("~/torque_mode", Cia402CommandInterfaces::, Cia402CommandInterfaces::); handle_set_target_service_ = createTriggerSrv("~/target", Cia402CommandInterfaces::, Cia402CommandInterfaces::); */ return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration Cia402DeviceController::command_interface_configuration() const { auto command_interfaces_config = CanopenProxyController::command_interface_configuration(); command_interfaces_config.names.push_back(joint_name_ + "/" + "init_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "init_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "halt_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "halt_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "recover_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "recover_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "position_mode_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "position_mode_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "velocity_mode_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "velocity_mode_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_velocity_mode_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_velocity_mode_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_position_mode_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_position_mode_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "interpolated_position_mode_cmd"); command_interfaces_config.names.push_back(joint_name_ + "/" + "interpolated_position_mode_fbk"); return command_interfaces_config; } controller_interface::InterfaceConfiguration Cia402DeviceController::state_interface_configuration() const { auto state_interfaces_config = CanopenProxyController::state_interface_configuration(); // no new state interfaces for this controller - additional state interfaces in cia402_system // are position and velocity which are claimed by joint_state_broadcaster and // feedback based controllers return state_interfaces_config; } controller_interface::CallbackReturn Cia402DeviceController::on_configure( const rclcpp_lifecycle::State & previous_state) { if (CanopenProxyController::on_configure(previous_state) != CallbackReturn::SUCCESS) return CallbackReturn::ERROR; return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn Cia402DeviceController::on_activate( const rclcpp_lifecycle::State & previous_state) { if (CanopenProxyController::on_activate(previous_state) != CallbackReturn::SUCCESS) return CallbackReturn::ERROR; return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn Cia402DeviceController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { if (CanopenProxyController::on_deactivate(previous_state) != CallbackReturn::SUCCESS) return CallbackReturn::ERROR; return CallbackReturn::SUCCESS; } controller_interface::return_type Cia402DeviceController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { if (CanopenProxyController::update(time, period) != controller_interface::return_type::OK) return controller_interface::return_type::ERROR; return controller_interface::return_type::OK; } } // namespace canopen_ros2_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( canopen_ros2_controllers::Cia402DeviceController, controller_interface::ControllerInterface) ================================================ FILE: canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "test_canopen_proxy_controller.hpp" #include #include #include #include #include #include class CanopenProxyControllerTest : public CanopenProxyControllerFixture { }; // When there are many mandatory parameters, set all by default and remove one by one in a // parameterized test TEST_P(CanopenProxyControllerTestParameterizedParameters, one_parameter_is_missing) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } // TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, CanopenProxyControllerTestParameterizedParameters, ::testing::Values( std::make_tuple(std::string("joint"), rclcpp::ParameterValue(std::string({}))))); TEST_F(CanopenProxyControllerTest, joint_names_parameter_not_set) { SetUpController(false); ASSERT_TRUE(controller_->joint_name_.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); ASSERT_TRUE(controller_->joint_name_.empty()); } TEST_F(CanopenProxyControllerTest, all_parameters_set_configure_success) { SetUpController(); ASSERT_TRUE(controller_->joint_name_.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset auto msg = controller_->input_cmd_.readFromNonRT(); ASSERT_THAT(controller_->joint_name_, joint_name_); } TEST_F(CanopenProxyControllerTest, check_exported_intefaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { EXPECT_EQ(command_intefaces.names[i], joint_name_ + "/" + command_interface_names_[i]); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) { EXPECT_EQ(state_intefaces.names[i], joint_name_ + "/" + state_interface_names_[i]); } } TEST_F(CanopenProxyControllerTest, activate_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset auto msg = *(controller_->input_cmd_.readFromNonRT()); EXPECT_EQ(msg, nullptr); } TEST_F(CanopenProxyControllerTest, update_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } TEST_F(CanopenProxyControllerTest, deactivate_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } TEST_F(CanopenProxyControllerTest, reactivate_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); const auto tpdo_data_initial = controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_optional(); ASSERT_TRUE(tpdo_data_initial); ASSERT_DOUBLE_EQ(*tpdo_data_initial, 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); const auto tpdo_data_after_deactivate = controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_optional(); ASSERT_TRUE(tpdo_data_after_deactivate); ASSERT_TRUE(std::isnan(*tpdo_data_after_deactivate)); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); const auto tpdo_data_after_activate = controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_optional(); ASSERT_TRUE(tpdo_data_after_activate); ASSERT_TRUE(std::isnan(*tpdo_data_after_activate)); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } TEST_F(CanopenProxyControllerTest, test_sequence_configure_activate) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); } TEST_F(CanopenProxyControllerTest, test_update_logic_fast) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // set command statically as value for good type std::shared_ptr msg = std::make_shared(); msg->index = 0u; msg->subindex = 0u; msg->data = 0u; controller_->input_cmd_.writeFromNonRT(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } ================================================ FILE: canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ #define CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ #include #include #include #include #include #include #include #include "canopen_ros2_controllers/canopen_proxy_controller.hpp" #include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" // TODO(anyone): replace the state and command message types using ControllerStateMsg = canopen_ros2_controllers::CanopenProxyController::ControllerNMTStateMsg; using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; using ControllerModeSrvType = canopen_ros2_controllers::CanopenProxyController::ControllerStartResetSrvType; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace // subclassing and friending so we can access member variables class TestableCanopenProxyController : public canopen_ros2_controllers::CanopenProxyController { FRIEND_TEST(CanopenProxyControllerTest, joint_names_parameter_not_set); FRIEND_TEST(CanopenProxyControllerTest, all_parameters_set_configure_success); FRIEND_TEST(CanopenProxyControllerTest, activate_success); FRIEND_TEST(CanopenProxyControllerTest, reactivate_success); FRIEND_TEST(CanopenProxyControllerTest, test_setting_slow_mode_service); FRIEND_TEST(CanopenProxyControllerTest, test_update_logic_fast); FRIEND_TEST(CanopenProxyControllerTest, test_update_logic_slow); public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { auto ret = canopen_ros2_controllers::CanopenProxyController::on_configure(previous_state); // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { cmd_subscriber_wait_set_.add_subscription(tpdo_subscriber_); } return ret; } /** * @brief wait_for_command blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. * * @return true if new ControllerCommandMsg msg was received, false if timeout. */ bool wait_for_command( rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; if (success) { executor.spin_some(); } return success; } bool wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { return wait_for_command(executor, cmd_subscriber_wait_set_, timeout); } // TODO(anyone): add implementation of any methods of your controller is needed private: rclcpp::WaitSet cmd_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers template class CanopenProxyControllerFixture : public ::testing::Test { public: static void SetUpTestCase() { rclcpp::init(0, nullptr); } void SetUp() { // initialize controller controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( "/test_canopen_ros2_controllers/commands", rclcpp::SystemDefaultsQoS()); service_caller_node_ = std::make_shared("service_caller"); slow_control_service_client_ = service_caller_node_->create_client( "/test_canopen_ros2_controllers/set_slow_control_mode"); } static void TearDownTestCase() { rclcpp::shutdown(); } void TearDown() { controller_.reset(nullptr); } protected: void SetUpController( bool set_parameters = true, std::string controller_name = "test_canopen_ros2_controllers") { ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); for (size_t i = 0; i < joint_command_values_.size(); ++i) { command_itfs_.emplace_back(hardware_interface::CommandInterface( joint_name_, command_interface_names_[i], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } std::vector state_ifs; state_itfs_.reserve(joint_state_values_.size()); state_ifs.reserve(joint_state_values_.size()); for (size_t i = 0; i < joint_state_values_.size(); ++i) { state_itfs_.emplace_back(hardware_interface::StateInterface( joint_name_, state_interface_names_[i], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); if (set_parameters) { controller_->get_node()->set_parameter({"joint", joint_name_}); } } void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( "/test_canopen_ros2_controllers/state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop rclcpp::WaitSet wait_set; // block used to wait on message wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if message has been received if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; // take message from subscription rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(msg, msg_info)); } // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( const std::vector & displacements = {0.45}, const std::vector & velocities = {0.0}, const double duration = 1.25) { auto wait_for_topic = [&](const auto topic_name) { size_t wait_count = 0; while (command_publisher_node_->count_subscribers(topic_name) == 0) { if (wait_count >= 5) { auto error_msg = std::string("publishing to ") + topic_name + " but no node subscribes to it"; throw std::runtime_error(error_msg); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); ++wait_count; } }; wait_for_topic(command_publisher_->get_topic_name()); ControllerCommandMsg msg; msg.index = 0u; msg.subindex = 0u; msg.data = 0u; command_publisher_->publish(msg); } protected: // Controller-related parameters std::string joint_name_ = {"joint1"}; std::vector command_interface_names_ = { "tpdo/index", "tpdo/subindex", "tpdo/data", "tpdo/ons", "nmt/reset", "nmt/reset_fbk", "nmt/start", "nmt/start_fbk"}; std::vector state_interface_names_ = { "rpdo/index", "rpdo/subindex", "rpdo/data", "nmt/state"}; // see StateInterfaces in canopen_proxy_controller.hpp for correct order; std::array joint_state_values_ = {0, 0, 0, 0}; // see CommandInterfaces in canopen_proxy_controller.hpp for correct order; std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101}; std::vector state_itfs_; std::vector command_itfs_; // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; rclcpp::Node::SharedPtr service_caller_node_; rclcpp::Client::SharedPtr slow_control_service_client_; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class CanopenProxyControllerTestParameterizedParameters : public CanopenProxyControllerFixture, public ::testing::WithParamInterface> { public: virtual void SetUp() { CanopenProxyControllerFixture::SetUp(); } static void TearDownTestCase() { CanopenProxyControllerFixture::TearDownTestCase(); } protected: void SetUpController(bool set_parameters = true) { CanopenProxyControllerFixture::SetUpController(set_parameters); controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); } }; #endif // CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ ================================================ FILE: canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp ================================================ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" TEST(TestLoadCanopenProxyController, load_controller) { rclcpp::init(0, nullptr); std::shared_ptr executor = std::make_shared(); controller_manager::ControllerManager cm( std::make_unique(), executor, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller( "test_canopen_ros2_controllers", "canopen_ros2_controllers/CanopenProxyController")); rclcpp::shutdown(); } ================================================ FILE: canopen_ros2_controllers/test/test_load_cia402_device_controller.cpp ================================================ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" TEST(TestLoadCanopenProxyController, load_controller) { rclcpp::init(0, nullptr); std::shared_ptr executor = std::make_shared(); controller_manager::ControllerManager cm( std::make_unique(), executor, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller( "test_canopen_ros2_controllers", "canopen_ros2_controllers/Cia402DeviceController")); rclcpp::shutdown(); } ================================================ FILE: canopen_ros2_controllers/test/test_load_cia402_robot_controller.cpp ================================================ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" TEST(TestLoadCanopenProxyController, load_controller) { rclcpp::init(0, nullptr); std::shared_ptr executor = std::make_shared(); controller_manager::ControllerManager cm( std::make_unique(), executor, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller( "test_canopen_ros2_controllers", "canopen_ros2_controllers/Cia402RobotController")); rclcpp::shutdown(); } ================================================ FILE: canopen_tests/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * Enhance SimpleSlave class with vendor ID parsing and adjust bus configuration timeouts * Multiple SDO types for slaves (`#278 `_) Co-authored-by: Kurtis Thrush Co-authored-by: Christoph Hellmann Santos * Contributors: Christoph Hellmann Santos 0.2.9 (2024-04-16) ------------------ 0.3.1 (2025-06-23) ------------------ 0.3.0 (2024-12-12) ------------------ * Add new line to get checking pipeline okay * Working version. * Periodic messages sent, but not received properly. 0.2.12 (2024-04-22) ------------------- * Merge pull request `#270 `_ from gsalinas/can-namespace-pr Put components loaded by the device container into its namespace, if any. * pre-commit update * Add node on namespaces. * Restore base version of canopen_system example. * Restore cia402_system launch to original. * Change cia402_system ros2_controllers back to original. * Add comment about the use of ReplaceString. * Push namespace to nodes in a group. * Separate out example of how to use a namespaced ros2_control/cia402 based system. * Remove commented-out arg and unneeded logging. * Update canopen test system launch to use a namespace. * Output robot_description_content to log during launch. * WIP adding namespace to cia402_system launch. * 0.2.9 * forthcoming * Contributors: Gerry Salinas, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ * Update package.xml * Solve buildfarm issues (`#155 `_) * Move exec deps from ros2_control to tests * Remove mkdir in install dir from cogen and dcfgen This causes a permission denied error on buildfarm. The install command creates it anyways * Contributors: Christoph Hellmann Santos 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Add trvivial integration test for robot_control * Add trivial integration tests for cia402_driver * Use the more tidy launch_test_node * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, James Ward, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_tests/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(canopen_tests) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(lely_core_libraries REQUIRED) generate_dcf(simple) generate_dcf(canopen_system) cogen_dcf(cia402_system) cogen_dcf(cia402_namespaced_system) cogen_dcf(cia402) generate_dcf(cia402_lifecycle) cogen_dcf(cia402_diagnostics) generate_dcf(simple_lifecycle) cogen_dcf(simple_diagnostics) generate_dcf(robot_control) install(DIRECTORY launch rviz urdf launch_tests DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) if(CANOPEN_ENABLED) find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(launch_tests/test_proxy_driver.py) add_launch_test(launch_tests/test_proxy_lifecycle_driver.py) endif() endif() ament_package() ================================================ FILE: canopen_tests/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_tests/README.md ================================================ # CANopen Tests Enable launch tests with --cmake-args -DCANOPEN_ENABLED. They can only be run on devices that have vcan0 enabled. ================================================ FILE: canopen_tests/ROS_NAMESPACES.md ================================================ The `cia402_namespaced_system.launch.py` example demonstrates how to run a CIA402 system in a namespace to allow multiple CANOpen-based robots in the same ROS domain. This example is necessary because the controller names are picked up from `ros2_controllers.yaml` and we must define their names in that file with a `__namespace__/controller_name:` key, then use ReplaceString from `nav2_common` to dynamically add the namespace to the controller definition. For a proxy system, no example is included; it's sufficient to include, e.g. the `proxy_setup.launch.py` launch description in another launch file and push a namespace onto it with PushROSNamespace in the ordinary way. ================================================ FILE: canopen_tests/config/canopen_system/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" proxy_device_1: node_id: 2 dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" proxy_device_2: node_id: 3 dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" ================================================ FILE: canopen_tests/config/canopen_system/ros2_controllers.yaml ================================================ controller_manager: ros__parameters: update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster node_1_controller: type: canopen_ros2_controllers/CanopenProxyController node_1_controller: ros__parameters: joint: node_1 ================================================ FILE: canopen_tests/config/canopen_system/simple.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=13 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1801 12=0x1A00 13=0x1A01 [ManufacturerObjects] SupportedObjects=5 1=0x4000 2=0x4001 3=0x4002 4=0x4003 5=0x4004 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1801] SubNumber=7 ParameterName=TPDO communication parameter 2 ObjectType=0x09 [1801sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1801sub1] ParameterName=COB-ID used by TPDO 2 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x280 [1801sub2] ParameterName=transmission type 2 DataType=0x0005 AccessType=rw DefaultValue=0xFF [1801sub3] ParameterName=inhibit time 2 DataType=0x0006 AccessType=rw [1801sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1801sub5] ParameterName=event timer 2 DataType=0x0006 AccessType=rw [1801sub6] ParameterName=SYNC start value 2 DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [1A01] ParameterName=TPDO mapping parameter 2 ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A01Value] NrOfEntries=1 1=0x40040020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 [4002] ParameterName=INTEGER32 test DataType=0x0004 AccessType=rw [4003] ParameterName=INTEGER16 test DataType=0x0003 AccessType=rw [4004] ParameterName=UNSIGNED32 sent from slave 2 DataType=0x0007 AccessType=rwr PDOMapping=1 ================================================ FILE: canopen_tests/config/cia402/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 revision_number: 0 boot_timeout_ms: 1000 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - {index: 0x60FF, sub_index: 0} # target velocity nodes: cia402_device_1: node_id: 2 namespace: "/test1" # cia402_device_2: # node_id: 3 # cia402_device_3: # node_id: 4 # cia402_device_4: # node_id: 5 # cia402_device_5: # node_id: 6 # cia402_device_6: # node_id: 7 ================================================ FILE: canopen_tests/config/cia402/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_tests/config/cia402_diagnostics/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" diagnostics: enable: true period: 1000 # in milliseconds revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - {index: 0x60FF, sub_index: 0} # target velocity nodes: cia402_device_1: node_id: 2 cia402_device_2: node_id: 3 cia402_device_3: node_id: 4 ================================================ FILE: canopen_tests/config/cia402_diagnostics/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_tests/config/cia402_lifecycle/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::LifecycleMasterDriver" package: "canopen_master_driver" sync_period: 20000 cia402_device_1: node_id: 2 dcf: "cia402_slave.eds" driver: "ros2_canopen::LifecycleCia402Driver" package: "canopen_402_driver" period: 20 enable_lazy_load: false revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - {index: 0x60FF, sub_index: 0} # target velocity ================================================ FILE: canopen_tests/config/cia402_lifecycle/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0xC0000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x40000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0xC0000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_tests/config/cia402_multichannel_system/README.md ================================================ # Multi-Channel CiA 402 System Configuration This configuration demonstrates the multi-channel/multi-axes feature as described in CiA 302-7 and CiA 402-2 specifications. ## Overview Multi-channel devices allow controlling multiple independent axes over a single CANopen Node-ID. This is useful for: - Multi-axis motor controllers (e.g., dual-axis servo drives) - Coordinated motion control systems - Reducing network complexity by consolidating multiple axes into one node ## CiA 402-2 Specification According to CiA 402-2, multi-axis devices use offset object dictionary indices: - **Channel 0** (standard): Indices 0x6000-0x67FF - **Channel 1**: Indices 0x6800-0x6FFF (offset +0x800) - **Channel 2**: Indices 0x7000-0x77FF (offset +0x1000) - **Channel N**: Indices 0x6000 + (N * 0x800) ### Common Object Examples | Object | Channel 0 | Channel 1 | Channel 2 | |--------|-----------|-----------|-----------| | Controlword | 0x6040 | 0x6840 | 0x7040 | | Statusword | 0x6041 | 0x6841 | 0x7041 | | Modes of operation | 0x6060 | 0x6860 | 0x7060 | | Position actual value | 0x6064 | 0x6864 | 0x7064 | | Velocity actual value | 0x606C | 0x686C | 0x706C | | Target position | 0x607A | 0x687A | 0x707A | | Target velocity | 0x60FF | 0x68FF | 0x70FF | ## Configuration Structure ### Bus Configuration (bus.yml) The bus configuration defines: 1. PDO mappings for each channel with appropriate index offsets 2. SDO initialization values for each channel 3. A single node_id that hosts multiple channels ### Hardware Description (URDF) When using ros2_control, the hardware interface must specify the channel for each joint: ```xml canopen_ros2_control/Cia402System $(find canopen_tests)/config/cia402_multichannel_system/bus.yml $(find canopen_tests)/config/master.dcf vcan0 2 0 2 1 ``` ### Key Parameters - **node_id**: The CANopen node ID of the device - **channel**: The channel/axis number within that device (0, 1, 2, ...) - Defaults to 0 if not specified (backward compatible) - Used to calculate the object dictionary index offset ## Backward Compatibility The multi-channel feature is fully backward compatible: - If the `channel` parameter is not specified, it defaults to 0 - Channel 0 uses standard object indices (no offset) - Existing single-axis configurations work without modification ## Usage Example 1. Configure the bus with appropriate PDO mappings for all channels 2. In your URDF, specify joints with their `node_id` and `channel` 3. Use standard ros2_control interfaces to control each axis independently 4. Each channel operates as an independent joint in the system ## Hardware Requirements Your CANopen device must: - Support CiA 402-2 multi-axis specification - Implement object dictionary entries at the correct offset indices - Have appropriate EDS/DCF file defining all channel objects ## Testing To test this configuration with simulated devices: ```bash # Set up virtual CAN interface sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set vcan0 txqueuelen 1000 sudo ip link set up vcan0 # Launch the multi-channel system ros2 launch canopen_tests cia402_multichannel_system_setup.launch.py ``` ## References - CiA 302-7: Framework for programmable CANopen devices - Part 7: Multi-axis positioning controller - CiA 402-2: CANopen device profile for drives and motion control - Part 2: Multiple axis communication parameter ================================================ FILE: canopen_tests/config/cia402_multichannel_system/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 # Multi-channel device configuration for CiA 402-2 # This example shows a device at node_id 2 with two channels (axes) nodes: # Single device with two channels cia402_multichannel_device: node_id: 2 driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" dcf: "cia402_slave_multichannel.eds" period: 10 revision_number: 0 # Multi-channel configuration num_channels: 2 # Number of axes/channels on this device # Optional: Custom names for each channel (used in joint_states topic) # If not provided, defaults to /0, /1, etc. channel_names: - "joint_0" - "joint_1" # Global scale factors and offsets (used as defaults for all channels) scale_pos_to_dev: 1000.0 # Convert position from SI units to device units scale_pos_from_dev: 0.001 # Convert position from device units to SI units scale_vel_to_dev: 1000.0 # Convert velocity from SI units to device units scale_vel_from_dev: 0.001 # Convert velocity from device units to SI units offset_pos_to_dev: 0.0 # Offset when converting position to device offset_pos_from_dev: 0.0 # Offset when converting position from device # Optional: Per-channel scale factors and offsets (override global settings) # If a channel is not listed, it will use the global values above channels: - # Channel 0 configuration scale_pos_to_dev: 1000.0 scale_pos_from_dev: 0.001 scale_vel_to_dev: 1000.0 scale_vel_from_dev: 0.001 offset_pos_to_dev: 0.0 offset_pos_from_dev: 0.0 - # Channel 1 configuration (different scale for this axis) scale_pos_to_dev: 2000.0 scale_pos_from_dev: 0.0005 scale_vel_to_dev: 2000.0 scale_vel_from_dev: 0.0005 offset_pos_to_dev: 10.0 offset_pos_from_dev: 5.0 # Other CIA 402 settings switching_state: 8 # Target state for mode switching (8 = Operation Enable) homing_timeout_seconds: 10 # Timeout for homing operation # Usage notes: # 1. Service naming depends on num_channels: # Single-channel (num_channels: 1): /node_name/init, /node_name/target # Multi-channel (num_channels: 2): /joint_0/init, /joint_1/target, etc. # # Examples for multi-channel: # ros2 service call /joint_0/target canopen_interfaces/srv/COTargetDouble "{target: 100.0}" # ros2 service call /joint_1/target canopen_interfaces/srv/COTargetDouble "{target: 50.0}" # ros2 service call /joint_0/init std_srvs/srv/Trigger # ros2 service call /joint_1/enable std_srvs/srv/Trigger # # 2. Joint state publishing: # Single-channel: name: ["node_name"] # Multi-channel: name: ["joint_0", "joint_1"] # Topic: ~/joint_states (sensor_msgs/msg/JointState) # - position: [pos_ch0, pos_ch1] # - velocity: [vel_ch0, vel_ch1] # # 3. Multi-channel services use channel_name as prefix: # - joint_0/init, joint_0/enable, joint_0/target, joint_0/position_mode, etc. # - joint_1/init, joint_1/enable, joint_1/target, joint_1/position_mode, etc. # # 4. Per-channel diagnostics are available with prefixes: ch0_, ch1_, etc. # Example CiA 402-2 Object Dictionary mapping: # Channel 0: Standard indices (0x6040, 0x6041, 0x6060, etc.) # Channel 1: Offset +0x800 (0x6840, 0x6841, 0x6860, etc.) # Channel 2: Offset +0x1000 (0x7040, 0x7041, 0x7060, etc.) # SDO configuration for both channels # Channel 0 (standard indices 0x6000-0x67FF) # Channel 1 (offset indices 0x6800-0x6FFF) sdo: # Channel 0 configuration - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} # Channel 1 configuration (indices offset by 0x800) - {index: 0x68C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x68C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6881, sub_index: 0, value: 1000} - {index: 0x6883, sub_index: 0, value: 2000} # TPDO configuration for both channels tpdo: # Channel 0 TPDOs 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word channel 0 - {index: 0x6061, sub_index: 0} # mode of operation display channel 0 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value channel 0 - {index: 0x606c, sub_index: 0} # velocity actual value channel 0 # Channel 1 TPDOs (indices offset by 0x800) 3: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6841, sub_index: 0} # status word channel 1 - {index: 0x6861, sub_index: 0} # mode of operation display channel 1 4: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6864, sub_index: 0} # position actual value channel 1 - {index: 0x686c, sub_index: 0} # velocity actual value channel 1 # RPDO configuration for both channels rpdo: # Channel 0 RPDOs 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword channel 0 - {index: 0x6060, sub_index: 0} # mode of operation channel 0 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position channel 0 - {index: 0x60FF, sub_index: 0} # target velocity channel 0 # Channel 1 RPDOs (indices offset by 0x800) 3: enabled: true cob_id: "auto" mapping: - {index: 0x6840, sub_index: 0} # controlword channel 1 - {index: 0x6860, sub_index: 0} # mode of operation channel 1 4: enabled: true cob_id: "auto" mapping: - {index: 0x687A, sub_index: 0} # target position channel 1 - {index: 0x68FF, sub_index: 0} # target velocity channel 1 ================================================ FILE: canopen_tests/config/cia402_multichannel_system/ros2_controllers.yaml ================================================ controller_manager: ros__parameters: update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster # Controller for channel 0 cia402_channel_0_controller: type: canopen_ros2_controllers/Cia402DeviceController # Controller for channel 1 cia402_channel_1_controller: type: canopen_ros2_controllers/Cia402DeviceController joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: forward_command_controller/ForwardCommandController cia402_channel_0_controller: ros__parameters: joint: node_2_channel_0 cia402_channel_1_controller: ros__parameters: joint: node_2_channel_1 forward_position_controller: ros__parameters: joints: - node_2_channel_0 - node_2_channel_1 interface_name: position joint_trajectory_controller: ros__parameters: joints: - node_2_channel_0 - node_2_channel_1 command_interfaces: - position state_interfaces: - position - velocity state_publish_rate: 100.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 node_2_channel_0: { trajectory: 0.2, goal: 0.1 } node_2_channel_1: { trajectory: 0.2, goal: 0.1 } ================================================ FILE: canopen_tests/config/cia402_namespaced_system/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - {index: 0x60FF, sub_index: 0} # target velocity nodes: cia402_device_1: node_id: 2 ================================================ FILE: canopen_tests/config/cia402_namespaced_system/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_tests/config/cia402_namespaced_system/ros2_controllers.yaml ================================================ __namespace__/controller_manager: ros__parameters: update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster cia402_device_1_controller: type: canopen_ros2_controllers/Cia402DeviceController joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: forward_command_controller/ForwardCommandController __namespace__/cia402_device_1_controller: ros__parameters: joint: node_1 __namespace__/forward_position_controller: ros__parameters: joints: - node_1 interface_name: position __namespace__/joint_trajectory_controller: ros__parameters: joints: - node_1 command_interfaces: - position state_interfaces: - position - velocity state_publish_rate: 100.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 node_1: { trajectory: 0.2, goal: 0.1 } ================================================ FILE: canopen_tests/config/cia402_system/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 defaults: dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position 3: enabled: false 4: enabled: false rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - {index: 0x60FF, sub_index: 0} # target velocity nodes: cia402_device_1: node_id: 2 ================================================ FILE: canopen_tests/config/cia402_system/cia402_slave.eds ================================================ [DeviceInfo] VendorName=ROS-Industrial VendorNumber=0x555 ProductName=CIA402VTD BaudRate_10=0 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 DynamicChannelsSupported=0 GroupMessaging=0 LSS_Supported=0 Granularity=8 SimpleBootUpSlave=1 SimpleBootUpMaster=0 NrOfRXPDO=4 NrOfTXPDO=4 [Comments] Lines=0 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [ManufacturerObjects] SupportedObjects=0 [OptionalObjects] SupportedObjects=67 1=0x1005 2=0x1008 3=0x1009 4=0x100A 5=0x100C 6=0x100D 7=0x1010 8=0x1011 9=0x1014 10=0x1015 11=0x1016 12=0x1017 13=0x1023 14=0x1029 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6040 32=0x6041 33=0x605A 34=0x605B 35=0x605C 36=0x605D 37=0x605E 38=0x6060 39=0x6061 40=0x6062 41=0x6063 42=0x6064 43=0x6065 44=0x6067 45=0x6068 46=0x606A 47=0x606C 48=0x607A 49=0x607C 50=0x607D 51=0x6081 52=0x6082 53=0x6083 54=0x6084 55=0x6085 56=0x608F 57=0x6098 58=0x6099 59=0x609A 60=0x60B0 61=0x60B1 62=0x60C2 63=0x60F2 64=0x60FD 65=0x60FF 66=0x6502 67=0x67FF [1000] ParameterName=Device Type ObjectType=0x07 DataType=0x0007 AccessType=const DefaultValue=0xFFFF0192 PDOMapping=0 [1001] ParameterName=Error Register ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1005] ParameterName=COB-ID SYNC ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 [1008] ParameterName=Manufacturer Device Name ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [1009] ParameterName=Manufacturer Hardware Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100A] ParameterName=Manufacturer Software Version ObjectType=0x07 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [100D] ParameterName=Life Time Factor ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1010] SubNumber=8 ParameterName=Store Parameter Field ObjectType=0x08 [1010sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1010sub1] ParameterName=Save all Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub2] ParameterName=Save Communication Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub3] ParameterName=Save Device Profile Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub4] ParameterName=Save Axis 0 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub5] ParameterName=Save Axis 1 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub6] ParameterName=Save Axis 2 Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1010sub7] ParameterName=Save Device Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] SubNumber=8 ParameterName=Restore Default Parameters ObjectType=0x08 [1011sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x7 PDOMapping=0 [1011sub1] ParameterName=Restore all Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub2] ParameterName=Restore Communication Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub3] ParameterName=Restore Device Profile Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub4] ParameterName=Restore Axis 0 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub5] ParameterName=Restore Axis 1 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1011sub6] ParameterName=Restore Axis 2 Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1011sub7] ParameterName=Restore Device Default Parameters ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [1014] ParameterName=COB-ID EMCY ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1015] ParameterName=Inhibit Time Emergency ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1016] SubNumber=2 ParameterName=Heartbeat Consumer Entries ObjectType=0x08 [1016sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x1 PDOMapping=0 [1016sub1] ParameterName=Consumer Heartbeat Time 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1018] SubNumber=5 ParameterName=Identity Object ObjectType=0x09 [1018sub0] ParameterName=number of entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x4 PDOMapping=0 [1018sub1] ParameterName=Vendor Id ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x555 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x0 PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=0 [1023] SubNumber=4 ParameterName=OS Command ObjectType=0x09 [1023sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x3 PDOMapping=0 [1023sub1] ParameterName=Command ObjectType=0x07 DataType=0x000A AccessType=rw PDOMapping=0 ObjFlags=0x00000001 [1023sub2] ParameterName=Status ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1023sub3] ParameterName=Reply ObjectType=0x07 DataType=0x000A AccessType=ro PDOMapping=0 [1029] SubNumber=3 ParameterName=Error Behaviour ObjectType=0x08 [1029sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=5 AccessType=ro DefaultValue=0x2 PDOMapping=0 [1029sub1] ParameterName=Communication Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1029sub2] ParameterName=Application Error ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1400] SubNumber=3 ParameterName=Receive PDO Communication Parameter 1 ObjectType=0x09 [1400sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1400sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000200 PDOMapping=0 [1400sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1401] SubNumber=3 ParameterName=Receive PDO Communication Parameter 2 ObjectType=0x09 [1401sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1401sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000300 PDOMapping=0 [1401sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1402] SubNumber=3 ParameterName=Receive PDO Communication Parameter 3 ObjectType=0x09 [1402sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1402sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000400 PDOMapping=0 [1402sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFF PDOMapping=0 [1403] SubNumber=3 ParameterName=Receive PDO Communication Parameter 4 ObjectType=0x09 [1403sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x02 PDOMapping=0 [1403sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000500 PDOMapping=0 [1403sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1600] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 1 ObjectType=0x09 [1600sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1600sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 2 ObjectType=0x09 [1601sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1601sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 3 ObjectType=0x09 [1602sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1602sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] SubNumber=4 ParameterName=Receive PDO Mapping Parameter 4 ObjectType=0x09 [1603sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1603sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 1 ObjectType=0x09 [1800sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1800sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000180 PDOMapping=0 [1800sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1800sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1800sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0 PDOMapping=0 [1801] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 2 ObjectType=0x09 [1801sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1801sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000280 PDOMapping=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1801sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1801sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 3 ObjectType=0x09 [1802sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1802sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000380 PDOMapping=0 [1802sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1802sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1802sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803] SubNumber=6 ParameterName=Transmit PDO Communication Parameter 4 ObjectType=0x09 [1803sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=ro DefaultValue=0x05 PDOMapping=0 [1803sub1] ParameterName=COB-ID ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000480 PDOMapping=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0xFE PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1803sub4] ParameterName=Compatibility Entry ObjectType=0x07 DataType=0x0005 AccessType=ro PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [1A00] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 1 ObjectType=0x09 [1A00sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x01 PDOMapping=0 [1A00sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 2 ObjectType=0x09 [1A01sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A01sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 3 ObjectType=0x09 [1A02sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A02sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] SubNumber=4 ParameterName=Transmit PDO Mapping Parameter 4 ObjectType=0x09 [1A03sub0] ParameterName=Number of Entries ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x02 PDOMapping=0 [1A03sub1] ParameterName=Mapping Entry 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapping Entry 2 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapping Entry 3 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6040] ParameterName=Controlword 1 ObjectType=0x07 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=Statusword 1 ObjectType=0x07 DataType=0x0006 AccessType=ro PDOMapping=1 [605A] ParameterName=Quick Stop Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0002 PDOMapping=0 [605B] ParameterName=Shutdown Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0000 PDOMapping=0 [605C] ParameterName=Disable Operation Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0001 PDOMapping=0 [605D] ParameterName=Halt Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw DefaultValue=0x0001 PDOMapping=0 [605E] ParameterName=Fault Reaction Option Code 1 ObjectType=0x07 DataType=0x0003 AccessType=ro DefaultValue=0x0002 PDOMapping=0 [6060] ParameterName=Modes of Operation 1 ObjectType=0x07 DataType=0x0002 AccessType=rww DefaultValue=0x00 PDOMapping=1 [6061] ParameterName=Modes of Operation Display 1 ObjectType=0x07 DataType=0x0002 AccessType=ro PDOMapping=1 [6062] ParameterName=Position Demand Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6063] ParameterName=Position Actual Internal Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=Position Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=Following Error Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6067] ParameterName=Position Window 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0xFFFFFFFF PDOMapping=0 [6068] ParameterName=Position Window Time 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x0000 PDOMapping=0 [606A] ParameterName=Sensor Selection Code 1 ObjectType=0x07 DataType=0x0003 AccessType=rw PDOMapping=0 [606C] ParameterName=Velocity Actual Value 1 ObjectType=0x07 DataType=0x0004 AccessType=ro PDOMapping=1 [607A] ParameterName=Target Position 1 ObjectType=0x07 DataType=0x0004 AccessType=rww DefaultValue=0x0 PDOMapping=1 [607C] ParameterName=Home Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [607D] SubNumber=3 ParameterName=Software Position Limit 1 ObjectType=0x08 [607Dsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [607Dsub1] ParameterName=Min Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=-2147483648 PDOMapping=0 [607Dsub2] ParameterName=Max Software Position Limit ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=2147483647 PDOMapping=0 [6081] ParameterName=Profile Velocity in pp-mode 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6082] ParameterName=End velocity 1 ObjectType=0x07 DataType=0x0007 AccessType=rw DefaultValue=0x0 PDOMapping=0 [6083] ParameterName=Profile Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6084] ParameterName=Profile Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6085] ParameterName=Quick Stop Deceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608F] SubNumber=3 ParameterName=Position Encoder Resolution 1 ObjectType=0x08 [608Fsub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x00000002 PDOMapping=0 [608Fsub1] ParameterName=Encoder Increments ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [608Fsub2] ParameterName=Motor Revolutions ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00000001 PDOMapping=0 [6098] ParameterName=Homing Method 1 ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=0x00 PDOMapping=0 [6099] SubNumber=3 ParameterName=Homing Speeds 1 ObjectType=0x08 [6099sub0] ParameterName=Highest sub-index supported ObjectType=0x07 DataType=5 AccessType=const DefaultValue=0x2 PDOMapping=0 [6099sub1] ParameterName=Fast Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [6099sub2] ParameterName=Slow Homing Speed ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [609A] ParameterName=Homing Acceleration 1 ObjectType=0x07 DataType=0x0007 AccessType=rw PDOMapping=0 [60B0] ParameterName=Position Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=0 [60B1] ParameterName=Velocity Offset 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60C2] SubNumber=3 ParameterName=Interpolation Time Period 1 ObjectType=0x09 [60C2sub0] ParameterName=NumOfEntries ObjectType=0x07 DataType=0x0005 AccessType=const DefaultValue=0x02 PDOMapping=0 [60C2sub1] ParameterName=timeUnits ObjectType=0x07 DataType=0x0005 AccessType=rw DefaultValue=0x1 PDOMapping=0 [60C2sub2] ParameterName=timeIndex ObjectType=0x07 DataType=0x0002 AccessType=rw DefaultValue=-2 PDOMapping=0 [60F2] ParameterName=Positioning Option Code 1 ObjectType=0x07 DataType=0x0006 AccessType=rw DefaultValue=0x00 PDOMapping=0 [60FD] ParameterName=Digital Inputs 1 ObjectType=0x07 DataType=0x0007 AccessType=ro PDOMapping=1 [60FF] ParameterName=Target Velocity 1 ObjectType=0x07 DataType=0x0004 AccessType=rw DefaultValue=0x0 PDOMapping=1 [6502] ParameterName=Supported Drive Modes 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x000000A5 PDOMapping=0 [67FF] ParameterName=Single Device Type 1 ObjectType=0x07 DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 ================================================ FILE: canopen_tests/config/cia402_system/ros2_controllers.yaml ================================================ controller_manager: ros__parameters: update_rate: 10 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster cia402_device_1_controller: type: canopen_ros2_controllers/Cia402DeviceController joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: forward_command_controller/ForwardCommandController cia402_device_1_controller: ros__parameters: joint: node_1 forward_position_controller: ros__parameters: joints: - node_1 interface_name: position joint_trajectory_controller: ros__parameters: joints: - node_1 command_interfaces: - position state_interfaces: - position - velocity state_publish_rate: 100.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 node_1: { trajectory: 0.2, goal: 0.1 } ================================================ FILE: canopen_tests/config/robot_control/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 joint_1: node_id: 2 dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 position_mode: 1 revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} - {index: 0x6060, sub_index: 0, value: 7} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position #- {index: 0x60FF, sub_index: 0} # target velocity joint_2: node_id: 3 dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 position_mode: 1 revision_number: 0 switching_state: 2 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s - {index: 0x6081, sub_index: 0, value: 1000} - {index: 0x6083, sub_index: 0, value: 2000} - {index: 0x6060, sub_index: 0, value: 7} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position #- {index: 0x60FF, sub_index: 0} # target velocity ================================================ FILE: canopen_tests/config/robot_control/cia402_slave.eds ================================================ [FileInfo] CreatedBy=Eduard Prelipcean ModifiedBy=Graure Iulian - Robert Description=CANopen Technosoft CreationTime=01:51PM CreationDate=02-07-2022 ModificationTime=01:51PM ModificationDate=02-07-2022 FileName=iPOS.D.v1.04.eds FileVersion=0x01 FileRevision=0x05 EDSVersion=5 [DeviceInfo] VendorName=Technosoft VendorNumber=0x000001A3 ProductName=iPOS BaudRate_10=0 BaudRate_20=0 BaudRate_50=0 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=0 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=8 DynamicChannelsSupported=0 CompactPDO=0 GroupMessaging=0 NrOfRXPDO=4 NrOfTXPDO=4 LSS_Supported=1 [DummyUsage] Dummy0001=0 Dummy0002=0 Dummy0003=0 Dummy0004=0 Dummy0005=0 Dummy0006=0 Dummy0007=0 [Comments] Lines=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [1000] ParameterName=Device type ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0x60192 PDOMapping=0 [1001] ParameterName=Error register ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=0 [1018] ParameterName=Identity Object ObjectType=0x9 SubNumber=5 [1018sub0] ParameterName=number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=4 PDOMapping=0 LowLimit=1 HighLimit=4 [1018sub1] ParameterName=Vendor ID ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0x000001A3 PDOMapping=0 [1018sub2] ParameterName=Product Code ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 [1018sub3] ParameterName=Revision number ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 [1018sub4] ParameterName=Serial number ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 [OptionalObjects] SupportedObjects=102 1=0x1002 2=0x1003 3=0x1005 4=0x1006 5=0x1008 6=0x100A 7=0x100C 8=0x100D 9=0x1010 10=0x1011 11=0x1013 12=0x1014 13=0x1017 14=0x1200 15=0x1400 16=0x1401 17=0x1402 18=0x1403 19=0x1600 20=0x1601 21=0x1602 22=0x1603 23=0x1800 24=0x1801 25=0x1802 26=0x1803 27=0x1A00 28=0x1A01 29=0x1A02 30=0x1A03 31=0x6007 32=0x6040 33=0x6041 34=0x605A 35=0x605B 36=0x605C 37=0x605D 38=0x605E 39=0x6060 40=0x6061 41=0x6062 42=0x6063 43=0x6064 44=0x6065 45=0x6066 46=0x6067 47=0x6068 48=0x6069 49=0x606B 50=0x606C 51=0x606F 52=0x6071 53=0x6075 54=0x6077 55=0x607A 56=0x607B 57=0x607C 58=0x607D 59=0x607E 60=0x6081 61=0x6083 62=0x6085 63=0x6086 64=0x6087 65=0x6089 66=0x608A 67=0x608B 68=0x608C 69=0x608D 70=0x608E 71=0x608F 72=0x6091 73=0x6092 74=0x6093 75=0x6094 76=0x6096 77=0x6097 78=0x6098 79=0x6099 80=0x609A 81=0x60A2 82=0x60A8 83=0x60A9 84=0x60AA 85=0x60AB 86=0x60B8 87=0x60B9 88=0x60BA 89=0x60BB 90=0x60BC 91=0x60BD 92=0x60C0 93=0x60C1 94=0x60C2 95=0x60F2 96=0x60F4 97=0x60F8 98=0x60FC 99=0x60FD 100=0x60FE 101=0x60FF 102=0x6502 [1002] ParameterName=Manufacturer status register ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [1003] ParameterName=Error Field ObjectType=0x8 SubNumber=6 [1003sub0] ParameterName=Number of errors in history ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=254 [1003sub1] ParameterName=Standard error field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 [1003sub2] ParameterName=Standard error field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 [1003sub3] ParameterName=Standard error field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 [1003sub4] ParameterName=Standard error field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 [1003sub5] ParameterName=Standard error field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 [1005] ParameterName=COB-ID SYNC Message ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x80 PDOMapping=0 [1006] ParameterName=Communication cycle period ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 [1008] ParameterName=Manufacturer device name ObjectType=0x7 DataType=0x0009 AccessType=const DefaultValue=iPOS PDOMapping=0 [100A] ParameterName=Manufacturer software version ObjectType=0x7 DataType=0x0009 AccessType=const PDOMapping=0 [100C] ParameterName=Guard time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [100D] ParameterName=Life time factor ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 [1010] ParameterName=Store parameters ObjectType=0x8 SubNumber=2 [1010sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=1 PDOMapping=0 LowLimit=1 HighLimit=1 [1010sub1] ParameterName=Save all Parameters ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1011] ParameterName=Restore default parameters ObjectType=0x8 SubNumber=2 [1011sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=1 PDOMapping=0 LowLimit=1 HighLimit=1 [1011sub1] ParameterName=Restore all default parameters ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1013] ParameterName=High Resolution Time Stamp ObjectType=0x7 DataType=0x0007 AccessType=rwr DefaultValue=0x00000000 PDOMapping=1 [1014] ParameterName=COB-ID EMCY Message ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 PDOMapping=0 [1017] ParameterName=Producer Heartbeat Time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1200] ParameterName=Server SDO Parameter ObjectType=0x9 SubNumber=3 [1200sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=2 HighLimit=2 [1200sub1] ParameterName=COB-ID Client -> Server (rx) ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=$NODEID+0x600 PDOMapping=0 [1200sub2] ParameterName=COB-ID Server -> Client (tx) ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=$NODEID+0x580 PDOMapping=0 [1400] ParameterName=RPDO1 Communication Parameter ObjectType=0x9 SubNumber=3 [1400sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=2 HighLimit=5 [1400sub1] ParameterName=COB-ID RPDO1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 PDOMapping=0 [1400sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1401] ParameterName=RPDO2 Communication Parameter ObjectType=0x9 SubNumber=3 [1401sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=2 HighLimit=5 [1401sub1] ParameterName=COB-ID RPDO2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x300 PDOMapping=0 [1401sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1402] ParameterName=RPDO3 Communication Parameter ObjectType=0x9 SubNumber=3 [1402sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=2 HighLimit=5 [1402sub1] ParameterName=COB-ID RPDO3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x400 PDOMapping=0 [1402sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1403] ParameterName=RPDO4 Communication Parameter ObjectType=0x9 SubNumber=3 [1403sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=2 HighLimit=5 [1403sub1] ParameterName=COB-ID RPDO4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x500 PDOMapping=0 [1403sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1600] ParameterName=RPDO1 Mapping Parameter ObjectType=0x9 SubNumber=9 [1600sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 LowLimit=0 HighLimit=8 [1600sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1600sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1600sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601] ParameterName=RPDO2 Mapping Parameter ObjectType=0x9 SubNumber=9 [1601sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1601sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1601sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60600008 PDOMapping=0 [1601sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1601sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602] ParameterName=RPDO3 Mapping Parameter ObjectType=0x9 SubNumber=9 [1602sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1602sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1602sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x607A0020 PDOMapping=0 [1602sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1602sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603] ParameterName=RPDO4 Mapping Parameter ObjectType=0x9 SubNumber=9 [1603sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1603sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 [1603sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60FF0020 PDOMapping=0 [1603sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1603sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1800] ParameterName=TPDO1 Communication Parameter ObjectType=0x9 SubNumber=5 [1800sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=2 HighLimit=5 [1800sub1] ParameterName=COB-ID TPDO1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 PDOMapping=0 [1800sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1800sub3] ParameterName=Inhibit Time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x012C PDOMapping=0 [1800sub5] ParameterName=Event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1801] ParameterName=TPDO2 Communication Parameter ObjectType=0x9 SubNumber=5 [1801sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=2 HighLimit=5 [1801sub1] ParameterName=COB-ID TPDO2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x280 PDOMapping=0 [1801sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1801sub3] ParameterName=Inhibit Time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x012C PDOMapping=0 [1801sub5] ParameterName=Event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1802] ParameterName=TPDO3 Communication Parameter ObjectType=0x9 SubNumber=5 [1802sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=2 HighLimit=5 [1802sub1] ParameterName=COB-ID TPDO3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000380 PDOMapping=0 [1802sub2] ParameterName=Transmision type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1802sub3] ParameterName=Inhibit Time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x012C PDOMapping=0 [1802sub5] ParameterName=Event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1803] ParameterName=TPDO4 Communication Parameter ObjectType=0x9 SubNumber=5 [1803sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=2 HighLimit=5 [1803sub1] ParameterName=COB-ID TPDO4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80000480 PDOMapping=0 [1803sub2] ParameterName=Transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 [1803sub3] ParameterName=Inhibit Time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x012C PDOMapping=0 [1803sub5] ParameterName=Event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1A00] ParameterName=TPDO1 Mapping Parameter ObjectType=0x9 SubNumber=9 [1A00sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 LowLimit=0 HighLimit=8 [1A00sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A00sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A00sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01] ParameterName=TPDO2 Mapping Parameter ObjectType=0x9 SubNumber=9 [1A01sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1A01sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A01sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60610008 PDOMapping=0 [1A01sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A01sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02] ParameterName=TPDO3 Mapping Parameter ObjectType=0x9 SubNumber=9 [1A02sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1A02sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A02sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 [1A02sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A02sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03] ParameterName=TPDO4 Mapping Parameter ObjectType=0x9 SubNumber=9 [1A03sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 [1A03sub1] ParameterName=Mapped object #1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 [1A03sub2] ParameterName=Mapped object #2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x606C0020 PDOMapping=0 [1A03sub3] ParameterName=Mapped object #3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03sub4] ParameterName=Mapped object #4 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03sub5] ParameterName=Mapped object #5 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03sub6] ParameterName=Mapped object #6 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03sub7] ParameterName=Mapped object #7 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [1A03sub8] ParameterName=Mapped object #8 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 [6007] ParameterName=Abort connection option code ObjectType=0x7 DataType=0x0003 AccessType=rww DefaultValue=0 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [6040] ParameterName=Control word ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [6041] ParameterName=Status word ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [605A] ParameterName=Quick stop option code ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [605B] ParameterName=Shutdown option code ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [605C] ParameterName=Disable operation option code ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=1 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [605D] ParameterName=Halt option code ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=1 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [605E] ParameterName=Fault reaction option code ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=-32768 HighLimit=32767 [6060] ParameterName=Modes of Operation ObjectType=0x7 DataType=0x0002 AccessType=rww PDOMapping=1 LowLimit=-128 HighLimit=127 [6061] ParameterName=Modes of Operation Display ObjectType=0x7 DataType=0x0002 AccessType=ro PDOMapping=1 LowLimit=-128 HighLimit=127 [6062] ParameterName=Position demand value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [6063] ParameterName=Position actual internal value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [6064] ParameterName=Position actual value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [6065] ParameterName=Following error window ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6066] ParameterName=Following error time out ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [6067] ParameterName=Position window ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6068] ParameterName=Position window time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [6069] ParameterName=Velocity sensor actual value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [606B] ParameterName=Velocity demand value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [606C] ParameterName=Velocity actual value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [606F] ParameterName=Velocity threshold ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [6071] ParameterName=Target torque ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 [6075] ParameterName=Motor rated current ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 HighLimit=4294967295 [6077] ParameterName=Torque actual value ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [607A] ParameterName=Target position ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [607B] ParameterName=Position range limit ObjectType=0x8 SubNumber=3 [607Bsub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [607Bsub1] ParameterName=Min position range limit ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [607Bsub2] ParameterName=Max position range limit ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [607C] ParameterName=Home offset ObjectType=0x7 DataType=0x0004 AccessType=rww DefaultValue=0 PDOMapping=1 [607D] ParameterName=Software position limit ObjectType=0x8 SubNumber=3 [607Dsub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [607Dsub1] ParameterName=Minimal position limit ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [607Dsub2] ParameterName=Maximal position limit ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [607E] ParameterName=Polarity ObjectType=0x7 DataType=0x0005 AccessType=rww DefaultValue=0 PDOMapping=1 [6081] ParameterName=Profile velocity ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6083] ParameterName=Profile acceleration ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [6085] ParameterName=Quick stop deceleration ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [6086] ParameterName=Motion profile type ObjectType=0x7 DataType=0x0003 AccessType=rww DefaultValue=0 PDOMapping=1 [6087] ParameterName=Torque slope ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 HighLimit=4294967295 [6089] ParameterName=Position notation index ObjectType=0x7 DataType=0x0002 AccessType=rww DefaultValue=0 PDOMapping=1 LowLimit=-128 HighLimit=127 [608A] ParameterName=Position domension index ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=255 [608B] ParameterName=Velocity notation index ObjectType=0x7 DataType=0x0002 AccessType=rww DefaultValue=0 PDOMapping=1 LowLimit=-128 HighLimit=127 [608C] ParameterName=Velocity dimension index ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=255 [608D] ParameterName=Acceleration notation index ObjectType=0x7 DataType=0x0002 AccessType=rww PDOMapping=1 LowLimit=-128 HighLimit=127 [608E] ParameterName=Acceleration dimension index ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=255 [608F] ParameterName=Position encoder resolution ObjectType=0x8 SubNumber=3 [608Fsub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [608Fsub1] ParameterName=Encoder increments ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [608Fsub2] ParameterName=Motor revolutions ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6091] ParameterName=Gear ratio ObjectType=0x8 SubNumber=3 [6091sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6091sub1] ParameterName=Motor shaft revolutions ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6091sub2] ParameterName=Driving shaft revolutions ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6092] ParameterName=Feed constant ObjectType=0x8 SubNumber=3 [6092sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6092sub1] ParameterName=Feed ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6092sub2] ParameterName=Shaft revolutions ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6093] ParameterName=Position factor ObjectType=0x8 SubNumber=3 [6093sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6093sub1] ParameterName=Position factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6093sub2] ParameterName=Position factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6094] ParameterName=Velocity encoder factor ObjectType=0x8 SubNumber=3 [6094sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6094sub1] ParameterName=Velocity encoder factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6094sub2] ParameterName=Velocity encoder factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6096] ParameterName=Velocity factor ObjectType=0x8 SubNumber=3 [6096sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6096sub1] ParameterName=Velocity factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6096sub2] ParameterName=Velocity factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6097] ParameterName=Acceleration factor ObjectType=0x8 SubNumber=3 [6097sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6097sub1] ParameterName=Acceleration factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6097sub2] ParameterName=Acceleration factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [6098] ParameterName=Homing method ObjectType=0x7 DataType=0x0002 AccessType=rww DefaultValue=0 PDOMapping=1 [6099] ParameterName=Homing speed ObjectType=0x8 SubNumber=3 [6099sub0] ParameterName=number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [6099sub1] ParameterName=Speed during search for switch ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0 PDOMapping=1 [6099sub2] ParameterName=Speed during search for zero ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0 PDOMapping=1 [609A] ParameterName=Homing acceleration ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [60A2] ParameterName=Jerk factor ObjectType=0x8 SubNumber=3 [60A2sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [60A2sub1] ParameterName=Jerk factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [60A2sub2] ParameterName=Jerk factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [60A8] ParameterName=SI unit position ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [60A9] ParameterName=SI unit velocity ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [60AA] ParameterName=SI unit acceleration ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [60AB] ParameterName=SI Unit jerk ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=4294967295 [60B8] ParameterName=Touch probe function ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [60B9] ParameterName=Touch probe status ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 [60BA] ParameterName=Touch probe 1 positive edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [60BB] ParameterName=Touch probe 1 negative edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [60BC] ParameterName=Touch probe 2 positive edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [60BD] ParameterName=Touch probe 2 negative edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [60C0] ParameterName=Interpolation sub mode select ObjectType=0x7 DataType=0x0003 AccessType=rww DefaultValue=0 PDOMapping=1 LowLimit=-32768 HighLimit=32767 [60C1] ParameterName=Interpolation data record ObjectType=0x8 SubNumber=3 [60C1sub0] ParameterName=number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [60C1sub1] ParameterName=X1: the first parameter of ip function ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [60C1sub2] ParameterName=X2: the second parameter of ip function ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [60C2] ParameterName=Interpolation time period ObjectType=0x9 SubNumber=3 [60C2sub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [60C2sub1] ParameterName=Interpolation time period value ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 [60C2sub2] ParameterName=Interpolation time index ObjectType=0x7 DataType=0x0002 AccessType=rww PDOMapping=1 [60F2] ParameterName=Positioning option code ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [60F4] ParameterName=Following error actual value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [60F8] ParameterName=Max slippage ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [60FC] ParameterName=Position demand internal value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [60FD] ParameterName=Digital inputs ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=1 [60FE] ParameterName=Digital outputs ObjectType=0x8 SubNumber=3 [60FEsub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 LowLimit=1 HighLimit=2 [60FEsub1] ParameterName=Physical outputs ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0 PDOMapping=1 [60FEsub2] ParameterName=Bit mask ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0 PDOMapping=1 [60FF] ParameterName=Target velocity ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [6502] ParameterName=Supported drive modes ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0x001F0065 PDOMapping=1 [ManufacturerObjects] SupportedObjects=84 1=0x2000 2=0x2002 3=0x2003 4=0x2004 5=0x2005 6=0x2006 7=0x2009 8=0x2010 9=0x2012 10=0x2013 11=0x2017 12=0x2018 13=0x2019 14=0x201A 15=0x201B 16=0x201C 17=0x201D 18=0x2022 19=0x2023 20=0x2025 21=0x2026 22=0x2027 23=0x2045 24=0x2046 25=0x2047 26=0x2050 27=0x2051 28=0x2052 29=0x2053 30=0x2054 31=0x2055 32=0x2058 33=0x2060 34=0x2064 35=0x2065 36=0x2066 37=0x2067 38=0x2069 39=0x206A 40=0x206B 41=0x206C 42=0x206F 43=0x2070 44=0x2071 45=0x2072 46=0x2073 47=0x2074 48=0x2075 49=0x2076 50=0x2077 51=0x2079 52=0x207A 53=0x207B 54=0x207C 55=0x207D 56=0x207F 57=0x2081 58=0x2083 59=0x2084 60=0x2085 61=0x2086 62=0x2087 63=0x2088 64=0x208B 65=0x208C 66=0x208D 67=0x208E 68=0x208F 69=0x2090 70=0x2091 71=0x2092 72=0x20A0 73=0x2100 74=0x2101 75=0x2102 76=0x2103 77=0x2104 78=0x2105 79=0x2106 80=0x2107 81=0x2108 82=0x210B 83=0x210F 84=0x2110 [2000] ParameterName=Manufacturer Error Register ObjectType=0x7 DataType=0x0006 AccessType=ro DefaultValue=0 PDOMapping=1 LowLimit=0 HighLimit=65535 [2002] ParameterName=Detailed Error Register ObjectType=0x7 DataType=0x0006 AccessType=ro DefaultValue=0 PDOMapping=1 LowLimit=0 HighLimit=65535 [2003] ParameterName=Communication Error Register ObjectType=0x7 DataType=0x0006 AccessType=ro DefaultValue=0 PDOMapping=1 LowLimit=0 HighLimit=65535 [2004] ParameterName=COB-ID High resolution time stamp ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x100 PDOMapping=0 [2005] ParameterName=Max slippage time out ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=0 [2006] ParameterName=Call TML function ObjectType=0x7 DataType=0x0006 AccessType=wo PDOMapping=0 LowLimit=1 HighLimit=10 [2009] ParameterName=Detailed Error Register 2 ObjectType=0x7 DataType=0x0006 AccessType=ro DefaultValue=0 PDOMapping=1 LowLimit=0 HighLimit=65535 [2010] ParameterName=Master settings ObjectType=0x7 DataType=0x0006 AccessType=rww DefaultValue=0 PDOMapping=1 LowLimit=0 HighLimit=65535 [2012] ParameterName=Master resolution ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [2013] ParameterName=EGEAR multiplication factor ObjectType=0x9 SubNumber=3 [2013sub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [2013sub1] ParameterName=EGEAR ratio numerator (slave) ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 [2013sub2] ParameterName=EGEAR ratio denominator (master) ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [2017] ParameterName=Master actual position ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [2018] ParameterName=Master actual speed ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [2019] ParameterName=CAM table load adress ObjectType=0x7 DataType=0x0006 AccessType=rw PDOMapping=0 [201A] ParameterName=CAM table run adress ObjectType=0x7 DataType=0x0006 AccessType=rw PDOMapping=0 [201B] ParameterName=CAM offset ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=4294967295 [201C] ParameterName=External online reference ObjectType=0x7 DataType=0x0004 AccessType=rww DefaultValue=0 PDOMapping=1 [201D] ParameterName=External Reference Type ObjectType=0x7 DataType=0x0006 AccessType=rw PDOMapping=0 [2022] ParameterName=Control effort ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [2023] ParameterName=Jerk time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [2025] ParameterName=Stepper current in open-loop operation ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 LowLimit=-32768 HighLimit=32767 [2026] ParameterName=Stand-by current for stepper in open-loop operation ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 LowLimit=-32768 HighLimit=32767 [2027] ParameterName=Time out for stepper stand-by current ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [2045] ParameterName=Digital outputs status ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [2046] ParameterName=Analogue input: Reference ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [2047] ParameterName=Analogue input: Feedback ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [2050] ParameterName=Over-current protection level ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=32767 [2051] ParameterName=Over-current time out ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=0 LowLimit=0 HighLimit=65535 [2052] ParameterName=Motor nominal current ObjectType=0x7 DataType=0x0006 AccessType=rw PDOMapping=0 LowLimit=0 HighLimit=32767 [2053] ParameterName=I2t protection integrator limit ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 LowLimit=0 HighLimit=2147483647 [2054] ParameterName=I2t protection scaling factor ObjectType=0x7 DataType=0x0006 AccessType=rw PDOMapping=0 LowLimit=0 HighLimit=65535 [2055] ParameterName=Analogue input: DC-link voltage ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=65535 [2058] ParameterName=Analogue input for drive temperature ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=0 LowLimit=0 HighLimit=65535 [2060] ParameterName=Software version of TML application ObjectType=0x7 DataType=0x0009 AccessType=ro PDOMapping=0 [2064] ParameterName=Read/Write configuration register ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0x84 PDOMapping=1 LowLimit=0 HighLimit=2147483647 [2065] ParameterName=Write data at address set in 0x2064 ObjectType=0x7 DataType=0x0007 AccessType=wo PDOMapping=1 LowLimit=0 HighLimit=2147483647 [2066] ParameterName=Read data from address set in 0x2064 ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=1 [2067] ParameterName=Write data at specified address ObjectType=0x7 DataType=0x0007 AccessType=wo PDOMapping=1 [2069] ParameterName=Checksum configuration register ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [206A] ParameterName=Checksum read register ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=0 [206B] ParameterName=CAM input scaling factor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0x00010000 PDOMapping=1 [206C] ParameterName=CAM output scaling factor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=0x00010000 PDOMapping=1 [206F] ParameterName=Time notation index ObjectType=0x7 DataType=0x0002 AccessType=rww PDOMapping=1 LowLimit=-128 HighLimit=127 [2070] ParameterName=Time dimension index ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=255 [2071] ParameterName=Time factor ObjectType=0x8 SubNumber=3 [2071sub0] ParameterName=number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [2071sub1] ParameterName=Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 [2071sub2] ParameterName=Feed constant ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 [2072] ParameterName=Interpolated position mode status ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 [2073] ParameterName=Interpolated position buffer length ObjectType=0x7 DataType=0x0006 AccessType=wo DefaultValue=7 PDOMapping=0 [2074] ParameterName=Interpolated position buffer configuration ObjectType=0x7 DataType=0x0006 AccessType=wo PDOMapping=0 [2075] ParameterName=Position triggers ObjectType=0x8 SubNumber=5 [2075sub0] ParameterName=Number of sub-indexes ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=4 PDOMapping=0 [2075sub1] ParameterName=Position trigger 1 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2075sub2] ParameterName=Position trigger 2 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2075sub3] ParameterName=Position trigger 3 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2075sub4] ParameterName=Position trigger 4 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2076] ParameterName=Save current configuration ObjectType=0x7 DataType=0x0006 AccessType=wo PDOMapping=0 [2077] ParameterName=Execute TML program ObjectType=0x7 DataType=0x0006 AccessType=wo PDOMapping=0 [2079] ParameterName=Interpolated position initial position ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [207A] ParameterName=Interpolated Position 1st order time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [207B] ParameterName=Homing current threshold ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 [207C] ParameterName=Homing current threshold time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [207D] ParameterName=Dummy ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=255 [207F] ParameterName=Current limit ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 LowLimit=0 HighLimit=65535 [2081] ParameterName=Set/change the actual motor position ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=0 [2083] ParameterName=Encoder resolution ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2084] ParameterName=Stepper resolution ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2085] ParameterName=Position triggered outputs ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=0 LowLimit=0 HighLimit=65535 [2086] ParameterName=Speed Limitation for CSP ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=0 [2087] ParameterName=Motor Speed ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [2088] ParameterName=Motor Position ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [208B] ParameterName=Sin signal AD value ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [208C] ParameterName=Cos signal AD value ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [208D] ParameterName=Auxiliary encoder position ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [208E] ParameterName=Auxiliary Settings Register ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=0 [208F] ParameterName=Digital inputs 8b ObjectType=0x9 SubNumber=3 [208Fsub0] ParameterName=Copy of Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [208Fsub1] ParameterName=Copy of Digital inputs 8b Lo ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=1 [208Fsub2] ParameterName=Copy of Digital Inputs 8b Hi ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=1 [2090] ParameterName=Digital outputs 8b ObjectType=0x9 SubNumber=3 [2090sub0] ParameterName=Copy of Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [2090sub1] ParameterName=Copy of Physical outputs 8b ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 [2090sub2] ParameterName=Copy of Bit mask 8b ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 [2091] ParameterName=Lock EEPROM ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=0 LowLimit=0 HighLimit=255 [2092] ParameterName=User Variables ObjectType=0x8 SubNumber=5 [2092sub0] ParameterName=Number of sub-indexes ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=4 PDOMapping=0 [2092sub1] ParameterName=UserVar1 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2092sub2] ParameterName=UserVar2 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2092sub3] ParameterName=UserVar3 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [2092sub4] ParameterName=UserVar4 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [20A0] ParameterName=Load Position and Speed monitoring ObjectType=0x9 SubNumber=4 [20A0sub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=3 PDOMapping=0 [20A0sub1] ParameterName=Reserved ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [20A0sub2] ParameterName=Load Position Monitor ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [20A0sub3] ParameterName=Load Speed Monitor ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 LowLimit=-2147483648 HighLimit=2147483647 [2100] ParameterName=Number of Steps per Revolution ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [2101] ParameterName=Number of microSteps per Step ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [2102] ParameterName=Brake status ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=1 LowLimit=0 HighLimit=1 [2103] ParameterName=Number of encoder Counts per Revolution ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [2104] ParameterName=Auxiliary encoder function ObjectType=0x7 DataType=0x0005 AccessType=rww PDOMapping=1 [2105] ParameterName=Auxiliary encoder status ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=1 [2106] ParameterName=Auxiliary encoder captured position positive edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [2107] ParameterName=Auxiliary encoder captured position negative edge ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [2108] ParameterName=Filter variable 16 bit ObjectType=0x9 SubNumber=4 [2108sub0] ParameterName=Number of elements ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=3 PDOMapping=0 [2108sub1] ParameterName=16bit variable address ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [2108sub2] ParameterName=Filter strength ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [2108sub3] ParameterName=Filtered variable 16bit ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [210B] ParameterName=Auxiliary Settings Register 2 ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=0 [210F] ParameterName=Acceleration encoder factor ObjectType=0x8 SubNumber=3 [210Fsub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [210Fsub1] ParameterName=Acceleration encoder factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [210Fsub2] ParameterName=Acceleration encoder factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [2110] ParameterName=Jerk encoder factor ObjectType=0x8 SubNumber=3 [2110sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [2110sub1] ParameterName=Jerk encoder factor Numerator ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 [2110sub2] ParameterName=Jerk encoder factor Divisor ObjectType=0x7 DataType=0x0007 AccessType=rww DefaultValue=1 PDOMapping=1 HighLimit=4294967295 ================================================ FILE: canopen_tests/config/robot_control/prbt_0_1.dcf ================================================ [FileInfo] CreatedBy=Dirk Osswald, SCHUNK GmbH & Co. KG ModifiedBy=Pilz GmbH & Co. KG Description=PRBT Electronic Data Sheet CreationTime=01:33PM CreationDate=05-23-2013 ModificationTime=00:00PM ModificationDate=09-07-2018 FileName=prbt_0_1.dcf FileVersion=0x03 FileRevision=0x01 EDSVersion=4.0 [DeviceInfo] VendorName=Pilz GmbH & Co. KG OrderCode=0 BaudRate_10=0 BaudRate_20=0 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=0 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=8 DynamicChannelsSupported=0 CompactPDO=0 GroupMessaging=0 NrOfRXPDO=2 NrOfTXPDO=4 LSS_Supported=1 [DummyUsage] Dummy0001=0 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 [Comments] Lines=0 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [1000] ParameterName=Device type ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 ObjFlags=0x0 [1001] ParameterName=Error register ObjectType=0x7 DataType=0x0005 AccessType=ro PDOMapping=0 ObjFlags=0x0 [1018] ParameterName=Identity Object ObjectType=0x9 SubNumber=5 ObjFlags=0x0 [1018sub0] ParameterName=number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=4 PDOMapping=0 LowLimit=1 HighLimit=4 ObjFlags=0 [1018sub1] ParameterName=Vendor ID ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 ObjFlags=0 [1018sub2] ParameterName=Product code ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 ObjFlags=0 [1018sub3] ParameterName=Revision number ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 ObjFlags=0 [1018sub4] ParameterName=Serial number ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 ObjFlags=0 [OptionalObjects] SupportedObjects=62 1=0x1002 2=0x1003 3=0x1005 4=0x1008 5=0x1009 6=0x100A 7=0x100C 8=0x100D 9=0x1014 10=0x1016 11=0x1017 12=0x1200 13=0x1400 14=0x1401 15=0x1600 16=0x1601 17=0x1800 18=0x1801 19=0x1802 20=0x1803 21=0x1A00 22=0x1A01 23=0x1A02 24=0x1A03 25=0x20A0 26=0x6040 27=0x6041 28=0x6042 29=0x6043 30=0x6044 31=0x6046 32=0x6048 33=0x6049 34=0x604C 35=0x6060 36=0x6061 37=0x6062 38=0x6064 39=0x6065 40=0x606B 41=0x606C 42=0x6073 43=0x6075 44=0x6077 45=0x607A 46=0x607C 47=0x607D 48=0x6081 49=0x6083 50=0x6086 51=0x6098 52=0x60B2 53=0x60C0 54=0x60C1 55=0x60C2 56=0x60C3 57=0x60C4 58=0x60F4 59=0x60FD 60=0x60FE 61=0x6502 62=0x2060 [1002] ParameterName=Manufacturer Status Register ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=0 [1003] ParameterName=Pre-defined Error Field ObjectType=0x8 SubNumber=3 ObjFlags=0x0 [1003sub0] ParameterName=Number of Errors ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0 [1003sub1] ParameterName=Standard Error Field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 ObjFlags=2 [1003sub2] ParameterName=Standard Error Field ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0 PDOMapping=0 ObjFlags=2 [1005] ParameterName=COB-ID SYNC message ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x00000080 PDOMapping=0 ObjFlags=0x0 [1008] ParameterName=Manufacturer device name ObjectType=0x7 DataType=0x0009 AccessType=const PDOMapping=0 ObjFlags=0x0 [1009] ParameterName=Manufacturer hardware version ObjectType=0x7 DataType=0x0009 AccessType=const PDOMapping=0 ObjFlags=0x0 [100A] ParameterName=Manufacturer software version ObjectType=0x7 DataType=0x0009 AccessType=const PDOMapping=0 ObjFlags=0x0 [100C] ParameterName=Guard time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 ObjFlags=0x0 [100D] ParameterName=Life time factor ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 ObjFlags=0x0 [1014] ParameterName=COB-ID EMCY message ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x80 PDOMapping=0 ObjFlags=0x0 [1016] ParameterName=Consumer Heartbeat Time ObjectType=0x8 SubNumber=4 ObjFlags=0x0 [1016sub0] ParameterName=Number of Entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=3 PDOMapping=0 LowLimit=1 HighLimit=127 ObjFlags=0 [1016sub1] ParameterName=Consumer Heartbeat Time of Node-ID 1 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0 [1016sub2] ParameterName=Consumer Heartbeat Time of Node-ID 2 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0 [1016sub3] ParameterName=Consumer Heartbeat Time of Node-ID 3 ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0 [1017] ParameterName=Producer heartbeat time ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x00000000 PDOMapping=0 LowLimit=0 HighLimit=32767 ObjFlags=0x0 ParameterValue=100 [1200] ParameterName=1. Server SDO parameter ObjectType=0x9 SubNumber=3 ObjFlags=0x0 [1200sub0] ParameterName=Number of supported Entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 ObjFlags=0 [1200sub1] ParameterName=COB-ID Client -> Server (rx) ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=$NodeID + 0x600 PDOMapping=0 ObjFlags=0 [1200sub2] ParameterName=COB-ID Server -> Client (tx) ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=$NodeID + 0x580 PDOMapping=0 ObjFlags=0 [1400] ParameterName=1. Receive PDO parameter ObjectType=0x9 SubNumber=3 ObjFlags=0x0 [1400sub0] ParameterName=Largest Sub-Index supported ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 ObjFlags=0 [1400sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x200 PDOMapping=0 ObjFlags=0 ParameterValue=$NODEID+0x200 [1400sub2] ParameterName=Transmission Type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 ObjFlags=0 ParameterValue=0x1 [1401] ParameterName=2. Receive PDO Parameter ObjectType=0x9 SubNumber=3 ObjFlags=0x0 [1401sub0] ParameterName=Largest Sub-Index supported ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 ObjFlags=0 [1401sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x300 PDOMapping=0 ObjFlags=0 ParameterValue=$NODEID+0x300 [1401sub2] ParameterName=Transmission Type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=255 PDOMapping=0 ObjFlags=0 ParameterValue=0x1 [1600] ParameterName=1. Receive PDO Mapping ObjectType=0x9 SubNumber=9 ObjFlags=0x0 [1600sub0] ParameterName=Number of mapped Application Objects ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 ObjFlags=0 ParameterValue=3 [1600sub1] ParameterName=1. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60400010 PDOMapping=0 ObjFlags=0 ParameterValue=0x60400010 [1600sub2] ParameterName=2. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60c10120 PDOMapping=0 ObjFlags=0 ParameterValue=0x60420010 [1600sub3] ParameterName=3. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 ParameterValue=0x60c10120 [1600sub4] ParameterName=4. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1600sub5] ParameterName=5. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1600sub6] ParameterName=6. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1600sub7] ParameterName=7. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1600sub8] ParameterName=8. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601] ParameterName=2. Receive PDO mapping ObjectType=0x9 SubNumber=9 ObjFlags=0x0 [1601sub0] ParameterName=Number of mapped Application Objects ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=8 ObjFlags=0 ParameterValue=2 [1601sub1] ParameterName=1. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 ParameterValue=0x607a0020 [1601sub2] ParameterName=2. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 ParameterValue=0x60810020 [1601sub3] ParameterName=3. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601sub4] ParameterName=4. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601sub5] ParameterName=5. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601sub6] ParameterName=6. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601sub7] ParameterName=7. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1601sub8] ParameterName=8. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1800] ParameterName=1. Transmit PDO Parameter ObjectType=0x9 SubNumber=4 ObjFlags=0x0 [1800sub0] ParameterName=Largest Sub-Index supported ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 ObjFlags=0 [1800sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x180 PDOMapping=0 ObjFlags=0 ParameterValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 ObjFlags=0 ParameterValue=0x1 [1800sub5] ParameterName=event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=32767 ObjFlags=0 [1801] ParameterName=2. Transmit PDO Parameter ObjectType=0x9 SubNumber=4 ObjFlags=0x0 [1801sub0] ParameterName=Largest Sub-Index supported ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 ObjFlags=0 [1801sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x280 PDOMapping=0 ObjFlags=0 [1801sub2] ParameterName=Transmission Type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 ObjFlags=0 [1801sub5] ParameterName=event timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0 [1802] ParameterName=3. Transmit PDO Parameter ObjectType=0x9 SubNumber=4 [1802sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=0x02 HighLimit=0x06 [1802sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x380 PDOMapping=0 ObjFlags=0 ParameterValue=$NODEID+0x380 [1802sub2] ParameterName=Transmission Type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 ParameterValue=0x1 [1802sub5] ParameterName=Event Timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1803] ParameterName=4. Transmit PDO Parameter ObjectType=0x9 SubNumber=4 [1803sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=5 PDOMapping=0 LowLimit=0x02 HighLimit=0x06 [1803sub1] ParameterName=COB-ID used by PDO ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=$NodeID + 0x480 PDOMapping=0 ObjFlags=0 [1803sub2] ParameterName=Transmission Type ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=254 PDOMapping=0 [1803sub5] ParameterName=Event Timer ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [1A00] ParameterName=1. Transmit PDO Mapping ObjectType=0x9 SubNumber=9 ObjFlags=0x0 [1A00sub0] ParameterName=Number of mapped Application Objects ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=3 PDOMapping=0 LowLimit=0 HighLimit=8 ObjFlags=0 ParameterValue=2 [1A00sub1] ParameterName=1. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60410010 PDOMapping=0 ObjFlags=0 ParameterValue=0x60410010 [1A00sub2] ParameterName=2. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60770010 PDOMapping=0 ObjFlags=0 ParameterValue=0x60610008 [1A00sub3] ParameterName=3. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x60640020 PDOMapping=0 ObjFlags=0 [1A00sub4] ParameterName=4. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A00sub5] ParameterName=5. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A00sub6] ParameterName=6. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A00sub7] ParameterName=7. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A00sub8] ParameterName=8. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01] ParameterName=2. Transmit PDO Mapping ObjectType=0x9 SubNumber=9 ObjFlags=0x0 [1A01sub0] ParameterName=Number of mapped Application Objects ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=2 PDOMapping=0 LowLimit=0 HighLimit=8 ObjFlags=0 [1A01sub1] ParameterName=1. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x606b0020 PDOMapping=0 ObjFlags=0 [1A01sub2] ParameterName=2. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0x606c0020 PDOMapping=0 ObjFlags=0 [1A01sub3] ParameterName=3. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01sub4] ParameterName=4. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01sub5] ParameterName=5. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01sub6] ParameterName=6. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01sub7] ParameterName=7. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A01sub8] ParameterName=8. mapped Object ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ObjFlags=0 [1A02] ParameterName=3. Transmit PDO Mapping ObjectType=0x9 SubNumber=9 [1A02sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=8 ParameterValue=2 [1A02sub1] ParameterName=PDO Mapping Entry ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ParameterValue=0x60640020 [1A02sub2] ParameterName=PDO Mapping Entry_2 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 ParameterValue=0x606c0020 [1A02sub3] ParameterName=PDO Mapping Entry_3 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A02sub4] ParameterName=PDO Mapping Entry_4 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A02sub5] ParameterName=PDO Mapping Entry_5 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A02sub6] ParameterName=PDO Mapping Entry_6 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A02sub7] ParameterName=PDO Mapping Entry_7 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A02sub8] ParameterName=PDO Mapping Entry_8 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03] ParameterName=4. Transmit PDO Mapping ObjectType=0x9 SubNumber=9 [1A03sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=8 [1A03sub1] ParameterName=PDO Mapping Entry ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub2] ParameterName=PDO Mapping Entry_2 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub3] ParameterName=PDO Mapping Entry_3 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub4] ParameterName=PDO Mapping Entry_4 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub5] ParameterName=PDO Mapping Entry_5 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub6] ParameterName=PDO Mapping Entry_6 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub7] ParameterName=PDO Mapping Entry_7 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [1A03sub8] ParameterName=PDO Mapping Entry_8 ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [6040] ParameterName=controlword ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [6041] ParameterName=statusword ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=1 [6042] ParameterName=vl_target_velocity ObjectType=0x7 DataType=0x0003 AccessType=rww DefaultValue=0 PDOMapping=1 [6043] ParameterName=vl_velocity_demand ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [6044] ParameterName=vl_velocity_actual_value ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [6046] ParameterName=vl_velocity_min_max_amount ObjectType=0x8 SubNumber=3 [6046sub0] ParameterName=vl_velocity_min_max_amount_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [6046sub1] ParameterName=vl_velocity_min_max_amount_vl_velocity_min_amount ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6046sub2] ParameterName=vl_velocity_min_max_amount_vl_velocity_max_amount ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6048] ParameterName=vl_velocity_acceleration ObjectType=0x9 SubNumber=3 [6048sub0] ParameterName=vl_velocity_acceleration_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [6048sub1] ParameterName=vl_velocity_acceleration_delta_speed ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6048sub2] ParameterName=vl_velocity_acceleration_delta_time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [6049] ParameterName=vl_velocity_deceleration ObjectType=0x9 SubNumber=3 [6049sub0] ParameterName=vl_velocity_deceleration_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [6049sub1] ParameterName=vl_velocity_deceleration_delta_speed ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 [6049sub2] ParameterName=vl_velocity_deceleration_delta_time ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [604C] ParameterName=vl_dimension_factor ObjectType=0x8 SubNumber=3 [604Csub0] ParameterName=vl_dimension_factor_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [604Csub1] ParameterName=vl_dimension_factor_numerator ObjectType=0x7 DataType=0x0004 AccessType=rw DefaultValue=1 PDOMapping=0 [604Csub2] ParameterName=vl_dimension_factor_denominator ObjectType=0x7 DataType=0x0004 AccessType=rw DefaultValue=6000 PDOMapping=0 [6060] ParameterName=modes_of_operation ObjectType=0x7 DataType=0x0002 AccessType=rw PDOMapping=1 ParameterValue=7 [6061] ParameterName=modes_of_operation_display ObjectType=0x7 DataType=0x0002 AccessType=ro PDOMapping=1 ObjFlags=0x1 [6062] ParameterName=position_demand_value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [6064] ParameterName=position_actual_value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [6065] ParameterName=following_error_window ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [606B] ParameterName=velocity_demand_value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [606C] ParameterName=velocity_actual_value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 [6073] ParameterName=max_current ObjectType=0x7 DataType=0x0006 AccessType=rww PDOMapping=1 [6075] ParameterName=motor_rated_current ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [6077] ParameterName=torque_actual_value ObjectType=0x7 DataType=0x0003 AccessType=ro PDOMapping=1 [607A] ParameterName=target_position ObjectType=0x7 DataType=0x0004 AccessType=rw PDOMapping=1 [607C] ParameterName=home_offset ObjectType=0x7 DataType=0x0004 AccessType=rw DefaultValue=0 PDOMapping=0 [607D] ParameterName=software_position_limit ObjectType=0x8 SubNumber=3 [607Dsub0] ParameterName=software_position_limit_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [607Dsub1] ParameterName=software_position_limit_min_position_limit ObjectType=0x7 DataType=0x0004 AccessType=rw [607Dsub2] ParameterName=software_position_limit_max_position_limit ObjectType=0x7 DataType=0x0004 AccessType=rw [6081] ParameterName=profile_velocity ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=1 DefaultValue=0x2710 [6083] ParameterName=profile_acceleration ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=1 [6086] ParameterName=motion_profile_type ObjectType=0x7 DataType=0x0003 AccessType=rw PDOMapping=0 [6098] ParameterName=homing_method ObjectType=0x7 DataType=0x0002 AccessType=const DefaultValue=0 PDOMapping=0 LowLimit=-128 HighLimit=35 ParameterValue=0 [60B2] ParameterName=torque_offset ObjectType=0x7 DataType=0x0003 AccessType=rww PDOMapping=1 [60C0] ParameterName=interpolation_sub_mode_select ObjectType=0x7 DataType=0x0003 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=-1 HighLimit=0 ParameterValue=0 [60C1] ParameterName=interpolation_data_record ObjectType=0x8 SubNumber=2 [60C1sub0] ParameterName=interpolation_data_record_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=1 PDOMapping=0 [60C1sub1] ParameterName=interpolation_data_record_setpoint_1 ObjectType=0x7 DataType=0x0004 AccessType=rww PDOMapping=1 [60C2] ParameterName=interpolation_time_period ObjectType=0x9 SubNumber=3 [60C2sub0] ParameterName=interpolation_time_period_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [60C2sub1] ParameterName=interpolation_time_period_value ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=8 PDOMapping=0 ParameterValue=10 [60C2sub2] ParameterName=interpolation_time_period_index ObjectType=0x7 DataType=0x0002 AccessType=rw DefaultValue=-3 PDOMapping=0 [60C3] ParameterName=interpolation_sync_definition ObjectType=0x8 SubNumber=3 [60C3sub0] ParameterName=interpolation_sync_definition_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [60C3sub1] ParameterName=syncronize_on_group ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 [60C3sub2] ParameterName=ip_sync_every_n_event ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=1 PDOMapping=0 LowLimit=0 HighLimit=255 [60C4] ParameterName=interpolation_data_configuration ObjectType=0x9 SubNumber=7 [60C4sub0] ParameterName=interpolation_data_configuration_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=6 PDOMapping=0 [60C4sub1] ParameterName=interpolation_data_configuration_maximum_buffer_size ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=1 PDOMapping=0 [60C4sub2] ParameterName=interpolation_data_configuration_actual_buffer_size ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 [60C4sub3] ParameterName=interpolation_data_configuration_buffer_organization ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 LowLimit=0 HighLimit=1 [60C4sub4] ParameterName=interpolation_data_configuration_buffer_position ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0 PDOMapping=0 [60C4sub5] ParameterName=interpolation_data_configuration_size_of_data_record ObjectType=0x7 DataType=0x0005 AccessType=wo DefaultValue=1 PDOMapping=0 [60C4sub6] ParameterName=interpolation_data_configuration_buffer_clear ObjectType=0x7 DataType=0x0005 AccessType=wo DefaultValue=0 PDOMapping=0 [60F4] ParameterName=following_error_actual_value ObjectType=0x7 DataType=0x0004 AccessType=ro PDOMapping=1 ObjFlags=0x0 [60FD] ParameterName=digital_inputs ObjectType=0x7 DataType=0x0007 AccessType=ro PDOMapping=1 ObjFlags=0x2 [60FE] ParameterName=digital_outputs ObjectType=0x8 SubNumber=3 [60FEsub0] ParameterName=digital_outputs_highest_subindex ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 [60FEsub1] ParameterName=digital_outputs_physical_outputs ObjectType=0x7 DataType=0x0007 AccessType=rww PDOMapping=1 ObjFlags=2 [60FEsub2] ParameterName=digital_outputs_bit_mask ObjectType=0x7 DataType=0x0007 AccessType=rw PDOMapping=0 [6502] ParameterName=supported_drive_modes ObjectType=0x7 DataType=0x0007 AccessType=ro DefaultValue=0x00000043 PDOMapping=0 [ManufacturerObjects] SupportedObjects=29 1=0x2000 2=0x2001 3=0x2002 4=0x2003 5=0x2004 6=0x2007 7=0x2008 8=0x2009 9=0x200A 10=0x200B 11=0x200C 12=0x200D 13=0x200E 14=0x2010 15=0x2011 16=0x2012 17=0x2013 18=0x2020 19=0x2021 20=0x2022 21=0x2023 22=0x2024 23=0x2030 24=0x2031 25=0x2032 26=0x2033 27=0x2034 28=0x2041 29=0x2050 [2000] ParameterName=manufacturer_debug_flags ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=0 PDOMapping=0 ObjFlags=0x0 [2001] ParameterName=debug_values_int32 ObjectType=0x8 SubNumber=5 [2001sub0] ParameterName=nb_debug_values_int32 ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=4 PDOMapping=0 [2001sub1] ParameterName=debug_value_int32_1 ObjectType=0x7 DataType=0x0004 AccessType=rwr PDOMapping=1 [2001sub2] ParameterName=debug_value_int32_2 ObjectType=0x7 DataType=0x0004 AccessType=rwr PDOMapping=1 [2001sub3] ParameterName=debug_value_int32_3 ObjectType=0x7 DataType=0x0004 AccessType=rwr PDOMapping=1 [2001sub4] ParameterName=debug_value_int32_4 ObjectType=0x7 DataType=0x0004 AccessType=rwr PDOMapping=1 [2002] ParameterName=debug_values_float ObjectType=0x8 SubNumber=5 [2002sub0] ParameterName=nb_debug_values_float ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=4 PDOMapping=0 [2002sub1] ParameterName=debug_value_float_1 ObjectType=0x7 DataType=0x0008 AccessType=rwr PDOMapping=1 [2002sub2] ParameterName=debug_value_float_2 ObjectType=0x7 DataType=0x0008 AccessType=rwr PDOMapping=1 [2002sub3] ParameterName=debug_value_float_3 ObjectType=0x7 DataType=0x0008 AccessType=rwr PDOMapping=1 [2002sub4] ParameterName=debug_value_float_4 ObjectType=0x7 DataType=0x0008 AccessType=rwr PDOMapping=1 [2003] ParameterName=debug_message ObjectType=0x7 DataType=0x0009 AccessType=ro PDOMapping=1 [2004] ParameterName=logging ObjectType=0x9 SubNumber=3 ObjFlags=0x3 [2004sub0] ParameterName=nb_logging ObjectType=0x7 DataType=0x0005 AccessType=const DefaultValue=2 PDOMapping=0 ObjFlags=3 [2004sub1] ParameterName=logging_state ObjectType=0x7 DataType=0x0006 AccessType=rw DefaultValue=0x8001 PDOMapping=0 ObjFlags=3 [2004sub2] ParameterName=logging_trigger_time ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 ObjFlags=3 [2007] ParameterName=EEPROM_Parameters ObjectType=0x9 SubNumber=2 [2007sub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=1 PDOMapping=0 [2007sub1] ParameterName=SECT_CONFIG ObjectType=0x7 DataType=0x000a AccessType=ro PDOMapping=0 ObjFlags=3 [2008] ParameterName=Password ObjectType=0x7 DataType=0x0009 AccessType=wo PDOMapping=0 ObjFlags=0x1 [2009] ParameterName=User ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=0 PDOMapping=0 [200A] ParameterName=Disconnect_Reset ObjectType=0x7 DataType=0x0005 AccessType=wo PDOMapping=0 ObjFlags=0x3 [200B] ParameterName=error_details ObjectType=0x9 SubNumber=4 DataType=0x0008 AccessType=ro [200Bsub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=3 PDOMapping=0 [200Bsub1] ParameterName=detail ObjectType=0x7 DataType=0x0008 AccessType=ro PDOMapping=0 [200Bsub2] ParameterName=file ObjectType=0x7 DataType=0x0009 AccessType=ro PDOMapping=0 ObjFlags=2 [200Bsub3] ParameterName=line ObjectType=0x7 DataType=0x0006 AccessType=ro PDOMapping=0 [200C] ParameterName=communication_mode ObjectType=0x7 DataType=0x0005 AccessType=rw PDOMapping=0 [200D] ParameterName=pcb_temperature ObjectType=0x9 SubNumber=3 [200Dsub0] ParameterName=Number of entries ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=2 PDOMapping=0 [200Dsub1] ParameterName=actual_pcb_temperature ObjectType=0x7 DataType=0x0008 AccessType=ro PDOMapping=1 [200Dsub2] ParameterName=pcb_temperature_update_period_ms ObjectType=0x7 DataType=0x0007 AccessType=rw DefaultValue=20000 PDOMapping=0 LowLimit=1000 HighLimit=3600000 [200E] ParameterName=sync_timeout_factor ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=6 PDOMapping=0 [2010] ParameterName=KR_Current ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2011] ParameterName=TN_Current ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2012] ParameterName=TD_Current ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2013] ParameterName=KC_Current ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2020] ParameterName=KR_Speed ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2021] ParameterName=TN_Speed ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2022] ParameterName=TD_Speed ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2023] ParameterName=KC_Speed ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2024] ParameterName=KS_FeedForward ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2030] ParameterName=KR_Pos ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2031] ParameterName=TN_Pos ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2032] ParameterName=TD_Pos ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2033] ParameterName=KC_Pos ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2034] ParameterName=KP_FeedForward ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2041] ParameterName=Delta_Position ObjectType=0x7 DataType=0x0008 AccessType=rw PDOMapping=0 [2050] ParameterName=extended_status ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=0 PDOMapping=1 ObjFlags=0x1 [20A0] ParameterName=vendor_parameters_and_status ObjectType=0x9 SubNumber=8 [20A0sub7] ParameterName=run_permitted_status ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=0 PDOMapping=1 ObjFlags=0x1 [2060] ParameterName=brake_test ObjectType=0x9 SubNumber=4 [2060sub1] ParameterName=brake_test_duration ObjectType=0x7 DataType=0x0006 AccessType=ro DefaultValue=2500 PDOMapping=0 ObjFlags=1 ParameterValue=2500 [2060sub2] ParameterName=start_brake_test ObjectType=0x7 DataType=0x0005 AccessType=rw DefaultValue=0 PDOMapping=0 ParameterValue=0 [2060sub3] ParameterName=brake_test_status ObjectType=0x7 DataType=0x0005 AccessType=ro DefaultValue=0 PDOMapping=0 ParameterValue=0 ================================================ FILE: canopen_tests/config/robot_control/ros2_controllers.yaml ================================================ controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster robot_controller: type: canopen_ros2_controllers/Cia402RobotController forward_position_controller: type: forward_command_controller/ForwardCommandController robot_controller: ros__parameters: joints: - joint1 - joint2 operation_mode: 1 command_poll_freq: 5 forward_position_controller: ros__parameters: joints: - joint1 - joint2 interface_name: position # joint_trajectory_controller: # ros__parameters: # joints: # - prbt_joint_1 # - prbt_joint_2 # - prbt_joint_3 # - prbt_joint_4 # - prbt_joint_5 # - prbt_joint_6 # command_interfaces: # - position # state_interfaces: # - position # - velocity # state_publish_rate: 100.0 # action_monitor_rate: 20.0 # allow_partial_joints_goal: false # constraints: # stopped_velocity_tolerance: 0.2 # goal_time: 0.6 # stopped_velocity_tolerance: 0.05 # prbt_joint_1: {trajectory: 0.157, goal: 0.01} # prbt_joint_2: {trajectory: 0.157, goal: 0.01} # prbt_joint_3: {trajectory: 0.157, goal: 0.01} # prbt_joint_4: {trajectory: 0.157, goal: 0.01} # prbt_joint_5: {trajectory: 0.157, goal: 0.01} # prbt_joint_6: {trajectory: 0.157, goal: 0.01} ================================================ FILE: canopen_tests/config/simple/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" proxy_device_1: node_id: 2 dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: true period: 10 boot_timeout_ms: 1000 # namespace: "/test1" proxy_device_2: node_id: 3 dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: true period: 10 boot_timeout_ms: 1000 # namespace: "/test2" ================================================ FILE: canopen_tests/config/simple/simple.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=11 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1A00 [ManufacturerObjects] SupportedObjects=5 1=0x4000 2=0x4001 3=0x4002 4=0x4003 5=0x4004 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 [4002] ParameterName=INTEGER32 test DataType=0x0004 AccessType=rw [4003] ParameterName=INTEGER16 test DataType=0x0003 AccessType=rw [4004] ParameterName=Test Data ObjectType=0x9 SubNumber=0x8 [4004sub0] ParameterName=Highest sub-index supported ObjectType=0x7 DataType=0x0005 AccessType=ro [4004sub1] ParameterName=BOOL test data ObjectType=0x7 DataType=0x0001 AccessType=rw [4004sub2] ParameterName=SIGNED8 test data ObjectType=0x7 DataType=0x0002 AccessType=rw [4004sub3] ParameterName=SIGNED16 test data ObjectType=0x7 DataType=0x0003 AccessType=rw [4004sub4] ParameterName=SIGNED32 test data ObjectType=0x7 DataType=0x0004 AccessType=rw [4004sub5] ParameterName=UNSIGNED8 test data ObjectType=0x7 DataType=0x0005 AccessType=rw [4004sub6] ParameterName=UNSIGNED16 test data ObjectType=0x7 DataType=0x0006 AccessType=rw [4004sub7] ParameterName=UNSIGNED32 test data ObjectType=0x7 DataType=0x0007 AccessType=rw ================================================ FILE: canopen_tests/config/simple_diagnostics/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" defaults: dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: true period: 10 diagnostics: enable: true period: 1000 # in milliseconds nodes: proxy_device_1: node_id: 2 proxy_device_2: node_id: 3 ================================================ FILE: canopen_tests/config/simple_diagnostics/simple.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=11 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1A00 [ManufacturerObjects] SupportedObjects=4 1=0x4000 2=0x4001 3=0x4002 4=0x4003 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 [4002] ParameterName=INTEGER32 test DataType=0x0004 AccessType=rw [4003] ParameterName=INTEGER16 test DataType=0x0003 AccessType=rw ================================================ FILE: canopen_tests/config/simple_lifecycle/bus.yml ================================================ options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::LifecycleMasterDriver" package: "canopen_master_driver" proxy_device_1: node_id: 2 dcf: "simple.eds" driver: "ros2_canopen::LifecycleProxyDriver" package: "canopen_proxy_driver" proxy_device_2: node_id: 3 dcf: "simple.eds" driver: "ros2_canopen::LifecycleProxyDriver" package: "canopen_proxy_driver" ================================================ FILE: canopen_tests/config/simple_lifecycle/simple.eds ================================================ [DeviceInfo] VendorName=Lely Industries N.V. VendorNumber=0x00000360 ProductName= ProductNumber=0x00000000 RevisionNumber=0x00000000 OrderCode= BaudRate_10=1 BaudRate_20=1 BaudRate_50=1 BaudRate_125=1 BaudRate_250=1 BaudRate_500=1 BaudRate_800=1 BaudRate_1000=1 SimpleBootUpMaster=0 SimpleBootUpSlave=1 Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 NrOfTxPDO=1 LSS_Supported=1 [DummyUsage] Dummy0001=1 Dummy0002=1 Dummy0003=1 Dummy0004=1 Dummy0005=1 Dummy0006=1 Dummy0007=1 Dummy0010=1 Dummy0011=1 Dummy0012=1 Dummy0013=1 Dummy0014=1 Dummy0015=1 Dummy0016=1 Dummy0018=1 Dummy0019=1 Dummy001A=1 Dummy001B=1 [MandatoryObjects] SupportedObjects=3 1=0x1000 2=0x1001 3=0x1018 [OptionalObjects] SupportedObjects=11 1=0x1003 2=0x1005 3=0x1014 4=0x1015 5=0x1016 6=0x1017 7=0x1029 8=0x1400 9=0x1600 10=0x1800 11=0x1A00 [ManufacturerObjects] SupportedObjects=2 1=0x4000 2=0x4001 [1000] ParameterName=Device type DataType=0x0007 AccessType=ro DefaultValue=0x00000000 [1001] ParameterName=Error register DataType=0x0005 AccessType=ro [1003] ParameterName=Pre-defined error field ObjectType=0x08 DataType=0x0007 AccessType=ro CompactSubObj=254 [1005] ParameterName=COB-ID SYNC message DataType=0x0007 AccessType=rw DefaultValue=0x00000080 [1014] ParameterName=COB-ID EMCY DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x80 [1015] ParameterName=Inhibit time EMCY DataType=0x0006 AccessType=rw DefaultValue=0 [1016] ParameterName=Consumer heartbeat time ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1017] ParameterName=Producer heartbeat time DataType=0x0006 AccessType=rw [1018] SubNumber=5 ParameterName=Identity object ObjectType=0x09 [1018sub0] ParameterName=Highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=4 [1018sub1] ParameterName=Vendor-ID DataType=0x0007 AccessType=ro DefaultValue=0x00000360 [1018sub2] ParameterName=Product code DataType=0x0007 AccessType=ro [1018sub3] ParameterName=Revision number DataType=0x0007 AccessType=ro [1018sub4] ParameterName=Serial number DataType=0x0007 AccessType=ro [1029] ParameterName=Error behavior object ObjectType=0x08 DataType=0x0005 AccessType=rw CompactSubObj=1 [1400] SubNumber=6 ParameterName=RPDO communication parameter ObjectType=0x09 [1400sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=5 [1400sub1] ParameterName=COB-ID used by RPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x200 [1400sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1400sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1400sub4] ParameterName=compatibility entry DataType=0x0005 AccessType=rw [1400sub5] ParameterName=event-timer DataType=0x0006 AccessType=rw [1600] ParameterName=RPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1600Value] NrOfEntries=1 1=0x40000020 [1800] SubNumber=7 ParameterName=TPDO communication parameter ObjectType=0x09 [1800sub0] ParameterName=highest sub-index supported DataType=0x0005 AccessType=const DefaultValue=6 [1800sub1] ParameterName=COB-ID used by TPDO DataType=0x0007 AccessType=rw DefaultValue=$NODEID+0x180 [1800sub2] ParameterName=transmission type DataType=0x0005 AccessType=rw DefaultValue=0xFF [1800sub3] ParameterName=inhibit time DataType=0x0006 AccessType=rw [1800sub4] ParameterName=reserved DataType=0x0005 AccessType=rw [1800sub5] ParameterName=event timer DataType=0x0006 AccessType=rw [1800sub6] ParameterName=SYNC start value DataType=0x0005 AccessType=rw [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 [1A00Value] NrOfEntries=1 1=0x40010020 [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 AccessType=rww PDOMapping=1 [4001] ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 ================================================ FILE: canopen_tests/launch/analyzers/cia402_diagnostic_analyzer.yaml ================================================ analyzers: ros__parameters: path: Aggregation cia402_device_1: type: diagnostic_aggregator/GenericAnalyzer path: Cia402Device1 contains: [ 'cia402_device_1' ] cia402_device_2: type: diagnostic_aggregator/GenericAnalyzer path: Cia402Device2 contains: [ 'cia402_device_2' ] cia402_device_3: type: diagnostic_aggregator/GenericAnalyzer path: Cia402Device3 contains: [ 'cia402_device_3' ] ================================================ FILE: canopen_tests/launch/analyzers/proxy_diagnostic_analyzer.yaml ================================================ analyzers: ros__parameters: path: Aggregation proxy_device_1: type: diagnostic_aggregator/GenericAnalyzer path: ProxyDevice1 contains: [ 'proxy_device_1' ] proxy_device_2: type: diagnostic_aggregator/GenericAnalyzer path: ProxyDevice2 contains: [ 'proxy_device_2' ] ================================================ FILE: canopen_tests/launch/canopen_system.launch.py ================================================ # Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # # * Neither the name of the {copyright_holder} nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Lovro Ivanov lovro.ivanov@gmail.com from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def launch_setup(context, *args, **kwargs): name = LaunchConfiguration("name") prefix = LaunchConfiguration("prefix") # bus configuration bus_config_package = LaunchConfiguration("bus_config_package") bus_config_directory = LaunchConfiguration("bus_config_directory") bus_config_file = LaunchConfiguration("bus_config_file") # bus configuration file full path bus_config = PathJoinSubstitution( [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] ) # master configuration master_config_package = LaunchConfiguration("master_config_package") master_config_directory = LaunchConfiguration("master_config_directory") master_config_file = LaunchConfiguration("master_config_file") # master configuration file full path master_config = PathJoinSubstitution( [FindPackageShare(master_config_package), master_config_directory, master_config_file] ) # can interface name can_interface_name = LaunchConfiguration("can_interface_name") # robot description stuff description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(description_package), "urdf", "canopen_system", description_file] ), " ", "name:=", name, " ", "prefix:=", prefix, " ", "bus_config:=", bus_config, " ", "master_config:=", master_config, " ", "can_interface_name:=", can_interface_name, " ", ] ) robot_description = {"robot_description": robot_description_content} # ros2 control configuration ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") # ros2 control configuration file full path ros2_control_config = PathJoinSubstitution( [ FindPackageShare(ros2_control_config_package), ros2_control_config_directory, ros2_control_config_file, ] ) # nodes to start are listed below control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_control_config], output="screen", ) # load one controller just to make sure it can connect to controller_manager joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) canopen_proxy_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["node_1_controller", "--controller-manager", "/controller_manager"], ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) # hardcoded slave configuration form test package slave_config = PathJoinSubstitution( [FindPackageShare(master_config_package), master_config_directory, "simple.eds"] ) slave_launch = PathJoinSubstitution( [FindPackageShare("canopen_fake_slaves"), "launch", "basic_slave.launch.py"] ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "2", "node_name": "slave_node_2", "slave_config": slave_config, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "3", "node_name": "slave_node_3", "slave_config": slave_config, }.items(), ) nodes_to_start = [ control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, slave_node_1, slave_node_2, canopen_proxy_controller_spawner, ] return nodes_to_start def generate_launch_description(): declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( "name", description="robot name", default_value="canopen_test_system" ) ) declared_arguments.append( DeclareLaunchArgument("prefix", description="Prefix.", default_value="") ) declared_arguments.append( DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", default_value="canopen_tests", ) ) declared_arguments.append( DeclareLaunchArgument( "description_file", description="Name of the urdf file.", default_value="canopen_system.urdf.xacro", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_package", default_value="canopen_tests", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_directory", default_value="config/canopen_system", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_file", default_value="ros2_controllers.yaml", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_package", default_value="canopen_tests", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_directory", default_value="config/canopen_system", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_file", default_value="bus.yml", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_package", default_value="canopen_tests", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_directory", default_value="config/canopen_system", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_file", default_value="master.dcf", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "can_interface_name", default_value="vcan0", description="Interface name for can", ) ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) ================================================ FILE: canopen_tests/launch/cia402_diagnostics_setup.launch.py ================================================ # Copyright 2023 ROS-Industrial # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch import launch_ros from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_diagnostics", "cia402_slave.eds", ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_eds_path, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "3", "node_name": "cia402_node_2", "slave_config": slave_eds_path, }.items(), ) slave_node_3 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "4", "node_name": "cia402_node_4", "slave_config": slave_eds_path, }.items(), ) master_bin_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_diagnostics", "master.bin", ) if not os.path.exists(master_bin_path): master_bin_path = "" device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_diagnostics", "master.dcf", ), "master_bin": master_bin_path, "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_diagnostics", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) diagnostics_analyzer_path = os.path.join( get_package_share_directory("canopen_tests"), "launch", "analyzers", "cia402_diagnostic_analyzer.yaml", ) diagnostics_aggregator_node = launch_ros.actions.Node( package="diagnostic_aggregator", executable="aggregator_node", output="screen", parameters=[diagnostics_analyzer_path], ) return LaunchDescription( [slave_node_1, slave_node_2, slave_node_3, device_container, diagnostics_aggregator_node] ) ================================================ FILE: canopen_tests/launch/cia402_lifecycle_setup.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_eds_path, }.items(), ) master_bin_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_lifecycle", "master.bin", ) if not os.path.exists(master_bin_path): master_bin_path = "" device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_lifecycle", "master.dcf", ), "master_bin": master_bin_path, "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_lifecycle", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) return LaunchDescription([slave_node_1, device_container]) ================================================ FILE: canopen_tests/launch/cia402_namespaced_system.launch.py ================================================ # Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # # * Neither the name of the {copyright_holder} nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Lovro Ivanov lovro.ivanov@gmail.com from launch import LaunchDescription import launch.actions from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from nav2_common.launch import ReplaceString from launch.actions import GroupAction from launch_ros.actions import PushROSNamespace def launch_setup(context, *args, **kwargs): name = LaunchConfiguration("name") prefix = LaunchConfiguration("prefix") bot_ns = LaunchConfiguration("bot_ns") # bus configuration bus_config_package = LaunchConfiguration("bus_config_package") bus_config_directory = LaunchConfiguration("bus_config_directory") bus_config_file = LaunchConfiguration("bus_config_file") # bus configuration file full path bus_config = PathJoinSubstitution( [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] ) # master configuration master_config_package = LaunchConfiguration("master_config_package") master_config_directory = LaunchConfiguration("master_config_directory") master_config_file = LaunchConfiguration("master_config_file") # master configuration file full path master_config = PathJoinSubstitution( [FindPackageShare(master_config_package), master_config_directory, master_config_file] ) # can interface name can_interface_name = LaunchConfiguration("can_interface_name") # robot description stuff description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(description_package), "urdf", "cia402_system", description_file] ), " ", "name:=", name, " ", "prefix:=", prefix, " ", "bus_config:=", bus_config, " ", "master_config:=", master_config, " ", "can_interface_name:=", can_interface_name, " ", ] ) robot_description = {"robot_description": robot_description_content} # ros2 control configuration ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") # ros2 control configuration file full path ros2_control_config = PathJoinSubstitution( [ FindPackageShare(ros2_control_config_package), ros2_control_config_directory, ros2_control_config_file, ] ) # Our controllers get their names from the control config, so we have to # include their namespace here to get them to start up within it. ros2_control_config = ReplaceString( source_file=ros2_control_config, replacements={"__namespace__/": (bot_ns, "/")} ) # nodes to start are listed below control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_control_config], output="screen", ) # load one controller just to make sure it can connect to controller_manager joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster"], ) cia402_device_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=[ "cia402_device_1_controller", ], ) forward_position_controller = Node( package="controller_manager", executable="spawner", arguments=[ "forward_position_controller", ], ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) # hardcoded slave configuration form test package slave_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/cia402", "cia402_slave.eds"] ) slave_launch = PathJoinSubstitution( [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_config, }.items(), ) namespaced_nodes = [ GroupAction( actions=[ PushROSNamespace(bot_ns), control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, slave_node_1, cia402_device_controller_spawner, forward_position_controller, ] ) ] return namespaced_nodes def generate_launch_description(): declared_arguments = [] declared_arguments.append( DeclareLaunchArgument("bot_ns", description="Namespace for these nodes", default_value="") ) declared_arguments.append( DeclareLaunchArgument( "name", description="robot name", default_value="canopen_test_system" ) ) declared_arguments.append( DeclareLaunchArgument("prefix", description="Prefix.", default_value="") ) declared_arguments.append( DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", default_value="canopen_tests", ) ) declared_arguments.append( DeclareLaunchArgument( "description_file", description="Name of the urdf file.", default_value="cia402_system.urdf.xacro", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_package", default_value="canopen_tests", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_directory", default_value="config/cia402_namespaced_system", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_file", default_value="ros2_controllers.yaml", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_package", default_value="canopen_tests", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_directory", default_value="config/cia402_system", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_file", default_value="bus.yml", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_package", default_value="canopen_tests", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_directory", default_value="config/cia402_system", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_file", default_value="master.dcf", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "can_interface_name", default_value="vcan0", description="Interface name for can", ) ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) ================================================ FILE: canopen_tests/launch/cia402_setup.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_eds_path, }.items(), ) master_bin_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402_lifecycle", "master.bin", ) if not os.path.exists(master_bin_path): master_bin_path = "" device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402", "master.dcf", ), "master_bin": master_bin_path, "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "cia402", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) return LaunchDescription([slave_node_1, device_container]) ================================================ FILE: canopen_tests/launch/cia402_system.launch.py ================================================ # Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # # * Neither the name of the {copyright_holder} nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Lovro Ivanov lovro.ivanov@gmail.com from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def launch_setup(context, *args, **kwargs): name = LaunchConfiguration("name") prefix = LaunchConfiguration("prefix") # bus configuration bus_config_package = LaunchConfiguration("bus_config_package") bus_config_directory = LaunchConfiguration("bus_config_directory") bus_config_file = LaunchConfiguration("bus_config_file") # bus configuration file full path bus_config = PathJoinSubstitution( [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] ) # master configuration master_config_package = LaunchConfiguration("master_config_package") master_config_directory = LaunchConfiguration("master_config_directory") master_config_file = LaunchConfiguration("master_config_file") # master configuration file full path master_config = PathJoinSubstitution( [FindPackageShare(master_config_package), master_config_directory, master_config_file] ) # can interface name can_interface_name = LaunchConfiguration("can_interface_name") # robot description stuff description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(description_package), "urdf", "cia402_system", description_file] ), " ", "name:=", name, " ", "prefix:=", prefix, " ", "bus_config:=", bus_config, " ", "master_config:=", master_config, " ", "can_interface_name:=", can_interface_name, " ", ] ) robot_description = {"robot_description": robot_description_content} # ros2 control configuration ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") # ros2 control configuration file full path ros2_control_config = PathJoinSubstitution( [ FindPackageShare(ros2_control_config_package), ros2_control_config_directory, ros2_control_config_file, ] ) # nodes to start are listed below control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_control_config], output="screen", ) # load one controller just to make sure it can connect to controller_manager joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) cia402_device_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["cia402_device_1_controller", "--controller-manager", "/controller_manager"], ) forward_position_controller = Node( package="controller_manager", executable="spawner", arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) # hardcoded slave configuration form test package slave_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/cia402", "cia402_slave.eds"] ) slave_launch = PathJoinSubstitution( [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_config, }.items(), ) nodes_to_start = [ control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, slave_node_1, cia402_device_controller_spawner, forward_position_controller, ] return nodes_to_start def generate_launch_description(): declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( "name", description="robot name", default_value="canopen_test_system" ) ) declared_arguments.append( DeclareLaunchArgument("prefix", description="Prefix.", default_value="") ) declared_arguments.append( DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", default_value="canopen_tests", ) ) declared_arguments.append( DeclareLaunchArgument( "description_file", description="Name of the urdf file.", default_value="cia402_system.urdf.xacro", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_package", default_value="canopen_tests", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_directory", default_value="config/cia402_system", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_file", default_value="ros2_controllers.yaml", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_package", default_value="canopen_tests", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_directory", default_value="config/cia402_system", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "bus_config_file", default_value="bus.yml", description="Path to bus configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_package", default_value="canopen_tests", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_directory", default_value="config/cia402_system", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "master_config_file", default_value="master.dcf", description="Path to master configuration file (*.dcf)", ) ) declared_arguments.append( DeclareLaunchArgument( "can_interface_name", default_value="vcan0", description="Interface name for can", ) ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) ================================================ FILE: canopen_tests/launch/proxy_diagnostics_setup.launch.py ================================================ # Copyright 2023 ROS-Industrial # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch import launch_ros from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_diagnostics", "simple.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_eds_path, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_eds_path, }.items(), ) print( os.path.join( get_package_share_directory("canopen_tests"), "config", "proxy_write_sdo", "master.dcf", ) ) device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_diagnostics", "master.dcf", ), "master_bin": "", "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_diagnostics", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) diagnostics_analyzer_path = os.path.join( get_package_share_directory("canopen_tests"), "launch", "analyzers", "proxy_diagnostic_analyzer.yaml", ) diagnostics_aggregator_node = launch_ros.actions.Node( package="diagnostic_aggregator", executable="aggregator_node", output="screen", parameters=[diagnostics_analyzer_path], ) return LaunchDescription( [slave_node_1, slave_node_2, device_container, diagnostics_aggregator_node] ) ================================================ FILE: canopen_tests/launch/proxy_lifecycle_setup.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_lifecycle", "simple.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_eds_path, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_eds_path, }.items(), ) print( os.path.join( get_package_share_directory("canopen_tests"), "config", "proxy_write_sdo", "master.dcf", ) ) device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_lifecycle", "master.dcf", ), "master_bin": "", "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple_lifecycle", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) return LaunchDescription([slave_node_1, slave_node_2, device_container]) ================================================ FILE: canopen_tests/launch/proxy_setup.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch import launch_ros from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "simple.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_eds_path, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_eds_path, }.items(), ) print( os.path.join( get_package_share_directory("canopen_tests"), "config", "proxy_write_sdo", "master.dcf", ) ) device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "master.dcf", ), "master_bin": "", "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) return LaunchDescription([slave_node_1, slave_node_2, device_container]) ================================================ FILE: canopen_tests/launch/proxy_setup_namespaced.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch import launch_ros from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): slave_eds_path = os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "simple.eds" ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_eds_path, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_eds_path, }.items(), ) print( os.path.join( get_package_share_directory("canopen_tests"), "config", "proxy_write_sdo", "master.dcf", ) ) device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "master.dcf", ), "master_bin": "", "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", "simple", "bus.yml", ), "can_interface_name": "vcan0", "namespace": "/test_ns", }.items(), ) return LaunchDescription([slave_node_1, slave_node_2, device_container]) ================================================ FILE: canopen_tests/launch/robot_control_setup.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [ FindPackageShare("canopen_tests"), "urdf/robot_controller", "robot_controller.urdf.xacro", ] ), ] ) robot_description = {"robot_description": robot_description_content} robot_control_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/robot_control", "ros2_controllers.yaml"] ) rviz_config_file = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "rviz", "robot_controller.rviz"] ) control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_control_config], output="screen", ) joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) robot_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["robot_controller", "--controller-manager", "/controller_manager"], ) forward_position_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], ) slave_config = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "config/robot_control", "cia402_slave.eds"] ) slave_launch = PathJoinSubstitution( [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_config, }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(slave_launch), launch_arguments={ "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_config, }.items(), ) nodes_to_start = [ # slave_node_2, # slave_node_1, control_node, joint_state_broadcaster_spawner, # robot_controller_spawner, forward_position_controller_spawner, robot_state_publisher_node, slave_node_1, slave_node_2, # rviz_node ] return LaunchDescription(nodes_to_start) ================================================ FILE: canopen_tests/launch/view_urdf.launch.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [ FindPackageShare("canopen_tests"), "urdf/robot_controller", "robot_controller.urdf.xacro", ] ), ] ) robot_description = {"robot_description": robot_description_content} rviz_config_file = PathJoinSubstitution( [FindPackageShare("canopen_tests"), "rviz", "robot_controller.rviz"] ) joint_state_publisher_node = Node( package="joint_state_publisher_gui", executable="joint_state_publisher_gui", ) robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], ) nodes_to_start = [ joint_state_publisher_node, robot_state_publisher_node, rviz_node, ] return LaunchDescription(nodes_to_start) ================================================ FILE: canopen_tests/launch_tests/test_cia402_driver.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from time import sleep import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import threading import rclpy from rclpy.node import Node from canopen_utils.launch_test_node import LaunchTestNode from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID, COTargetDouble from canopen_interfaces.msg import COData from std_srvs.srv import Trigger import unittest @pytest.mark.rostest def generate_test_description(): launch_desc = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), "/cia402_setup.launch.py", ] ) ) ready_to_test = launch.actions.TimerAction( period=5.0, actions=[launch_testing.actions.ReadyToTest()], ) return (LaunchDescription([launch_desc, ready_to_test]), {}) class TestSDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = CORead.Request() req.index = 0x1000 req.subindex = 0 res = CORead.Response() res.success = True res.data = 0xFFFF0192 self.node.call_service("cia402_device_1/sdo_read", CORead, req, res) def test_init(self): req = Trigger.Request() res = Trigger.Response() res.success = True self.node.call_service("cia402_device_1/init", Trigger, req, res) def test_position_mode(self): req = Trigger.Request() res = Trigger.Response() res.success = True self.node.call_service("cia402_device_1/init", Trigger, req, res) self.node.call_service("cia402_device_1/position_mode", Trigger, req, res) target_req = COTargetDouble.Request() target_req.target = 1.0 target_res = COTargetDouble.Response() target_res.success = True self.node.call_service("cia402_device_1/target", COTargetDouble, target_req, target_res) ================================================ FILE: canopen_tests/launch_tests/test_proxy_driver.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from time import sleep import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import threading import rclpy from rclpy.node import Node from canopen_utils.launch_test_node import LaunchTestNode from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID from canopen_interfaces.msg import COData from std_srvs.srv import Trigger import unittest @pytest.mark.rostest def generate_test_description(): launch_desc = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), "/proxy_setup.launch.py", ] ) ) ready_to_test = launch.actions.TimerAction( period=5.0, actions=[launch_testing.actions.ReadyToTest()], ) return (LaunchDescription([launch_desc, ready_to_test]), {}) class TestNMT(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUpClass(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDownClass(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_reset_nmt(self): request = Trigger.Request() response = Trigger.Response() response.success = True self.node.call_service("proxy_device_2/nmt_reset_node", Trigger, request, response) time.sleep(1) class TestSDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = CORead.Request() req.index = 0x4000 req.subindex = 0 res = CORead.Response() res.success = True res.data = 0 self.node.call_service("proxy_device_1/sdo_read", CORead, req, res) self.node.call_service("proxy_device_2/sdo_read", CORead, req, res) def test_sdo_write(self): index = 0x4000 subindex = 0 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_signed8(self): """Tests SDO writing to a SIGNED8 data object.""" index = 0x4004 subindex = 2 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_signed16(self): """Tests SDO writing to a SIGNED16 data object.""" index = 0x4004 subindex = 3 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_signed32(self): """Tests SDO writing to a SIGNED32 data object.""" index = 0x4004 subindex = 4 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_unsigned8(self): """Tests SDO writing to an UNSIGNED8 data object.""" index = 0x4004 subindex = 5 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_unsigned16(self): """Tests SDO writing to an UNSIGNED16 data object.""" index = 0x4004 subindex = 6 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) def test_sdo_write_unsigned32(self): """Tests SDO writing to an UNSIGNED32 data object.""" index = 0x4004 subindex = 7 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) class TestSDOMaster(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = COReadID.Request() req.index = 0x4000 req.subindex = 0 req.nodeid = 2 req.canopen_datatype = 0x7 res = COReadID.Response() res.success = True res.data = 0 self.node.call_service("/master/sdo_read", COReadID, req, res) req.nodeid = 3 self.node.call_service("/master/sdo_read", COReadID, req, res) def test_sdo_write(self): index = 0x4000 subindex = 0 data = 100 req = COWriteID.Request() req.index = index req.subindex = subindex req.data = data req.canopen_datatype = 0x7 req.nodeid = 2 res = COWriteID.Response() res.success = True rreq = COReadID.Request() rreq.index = index rreq.subindex = subindex rreq.nodeid = 2 rres = COReadID.Response() rres.success = True rres.data = data rreq.canopen_datatype = 0x7 self.node.call_service("master/sdo_write", COWriteID, req, res) self.node.call_service("master/sdo_write", COWriteID, req, res) self.node.call_service("master/sdo_read", COReadID, rreq, rres) self.node.call_service("master/sdo_read", COReadID, rreq, rres) req.data = 0 self.node.call_service("master/sdo_write", COWriteID, req, res) self.node.call_service("master/sdo_write", COWriteID, req, res) class TestPDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_pdo(self): msg = COData() msg.index = 0x4000 msg.subindex = 0 msg.data = 200 pub_msg = COData() pub_msg.index = 0x4001 pub_msg.subindex = 0 pub_msg.data = 200 thread = threading.Thread( target=self.node.subscribe_and_wait_for_message, args=["proxy_device_1/rpdo", COData, pub_msg], ) thread.start() self.node.publish_message("proxy_device_1/tpdo", COData, msg) time.sleep(0.1) msg.data = 0 self.node.publish_message("proxy_device_1/tpdo", COData, msg) thread.join() ================================================ FILE: canopen_tests/launch_tests/test_proxy_driver_namespaced.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from time import sleep import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import threading import rclpy from rclpy.node import Node from canopen_utils.launch_test_node import LaunchTestNode from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID from canopen_interfaces.msg import COData from std_srvs.srv import Trigger import unittest @pytest.mark.rostest def generate_test_description(): launch_desc = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), "/proxy_setup.launch.py", ] ) ) ready_to_test = launch.actions.TimerAction( period=5.0, actions=[launch_testing.actions.ReadyToTest()], ) return (LaunchDescription([launch_desc, ready_to_test]), {}) class TestNMT(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUpClass(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDownClass(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_reset_nmt(self): request = Trigger.Request() response = Trigger.Response() response.success = True self.node.call_service( "/test_ns/proxy_device_2/nmt_reset_node", Trigger, request, response ) time.sleep(1) class TestSDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = CORead.Request() req.index = 0x4000 req.subindex = 0 res = CORead.Response() res.success = True res.data = 0 self.node.call_service("/test_ns/proxy_device_1/sdo_read", CORead, req, res) self.node.call_service("/test_ns/proxy_device_2/sdo_read", CORead, req, res) def test_sdo_write(self): index = 0x4000 subindex = 0 data = 100 req = COWrite.Request() req.index = index req.subindex = subindex req.data = data res = COWrite.Response() res.success = True rreq = CORead.Request() rreq.index = index rreq.subindex = subindex rres = CORead.Response() rres.success = True rres.data = data self.node.call_service("/test_ns/proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("/test_ns/proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) self.node.call_service("/test_ns/proxy_device_1/sdo_read", CORead, rreq, rres) self.node.call_service("/test_ns/proxy_device_2/sdo_read", CORead, rreq, rres) req.data = 0 time.sleep(0.01) self.node.call_service("/test_ns/proxy_device_1/sdo_write", COWrite, req, res) self.node.call_service("/test_ns/proxy_device_2/sdo_write", COWrite, req, res) time.sleep(0.01) class TestSDOMaster(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = COReadID.Request() req.index = 0x4000 req.subindex = 0 req.nodeid = 2 req.canopen_datatype = 0x7 res = COReadID.Response() res.success = True res.data = 0 self.node.call_service("/test_ns/master/sdo_read", COReadID, req, res) req.nodeid = 3 self.node.call_service("/test_ns/master/sdo_read", COReadID, req, res) def test_sdo_write(self): index = 0x4000 subindex = 0 data = 100 req = COWriteID.Request() req.index = index req.subindex = subindex req.data = data req.canopen_datatype = 0x7 req.nodeid = 2 res = COWriteID.Response() res.success = True rreq = COReadID.Request() rreq.index = index rreq.subindex = subindex rreq.nodeid = 2 rres = COReadID.Response() rres.success = True rres.data = data rreq.canopen_datatype = 0x7 self.node.call_service("/test_ns/master/sdo_write", COWriteID, req, res) self.node.call_service("/test_ns/master/sdo_write", COWriteID, req, res) self.node.call_service("/test_ns/master/sdo_read", COReadID, rreq, rres) self.node.call_service("/test_ns/master/sdo_read", COReadID, rreq, rres) req.data = 0 self.node.call_service("/test_ns/master/sdo_write", COWriteID, req, res) self.node.call_service("/test_ns/master/sdo_write", COWriteID, req, res) class TestPDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_pdo(self): msg = COData() msg.index = 0x4000 msg.subindex = 0 msg.data = 200 pub_msg = COData() pub_msg.index = 0x4001 pub_msg.subindex = 0 pub_msg.data = 200 thread = threading.Thread( target=self.node.subscribe_and_wait_for_message, args=["/test_ns/proxy_device_1/rpdo", COData, pub_msg], ) thread.start() self.node.publish_message("/test_ns/proxy_device_1/tpdo", COData, msg) time.sleep(0.1) msg.data = 0 self.node.publish_message("/test_ns/proxy_device_1/tpdo", COData, msg) thread.join() ================================================ FILE: canopen_tests/launch_tests/test_proxy_lifecycle_driver.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from time import sleep import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import threading import rclpy from canopen_utils.launch_test_node import LaunchTestNode from lifecycle_msgs.msg import Transition from lifecycle_msgs.srv import ChangeState import unittest from canopen_interfaces.srv import CORead, COWrite from canopen_interfaces.msg import COData @pytest.mark.rostest def generate_test_description(): launch_desc = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), "/proxy_lifecycle_setup.launch.py", ] ) ) ready_to_test = launch.actions.TimerAction( period=5.0, actions=[launch_testing.actions.ReadyToTest()], ) return (LaunchDescription([launch_desc, ready_to_test]), {}) class TestLifecycle(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_configure_unconfigure(self): req = ChangeState.Request() req.transition.id = Transition.TRANSITION_CONFIGURE res = ChangeState.Response() res.success = True self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) def test_full_cycle(self): req = ChangeState.Request() res = ChangeState.Response() res.success = True req.transition.id = Transition.TRANSITION_CONFIGURE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CONFIGURE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_ACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************ACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_DEACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************DEACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CLEANUP SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CONFIGURE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CONFIGURE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_ACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************ACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_DEACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************DEACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CLEANUP SUCCESSFUL*************************") class TestSDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_full_cycle_sdo(self): req = ChangeState.Request() res = ChangeState.Response() res.success = True req.transition.id = Transition.TRANSITION_CONFIGURE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CONFIGURE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_ACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************ACTIVATE SUCCESSFUL*************************") data = 100 readreq = CORead.Request() readreq.index = 0x4000 readreq.subindex = 0x00 readres = CORead.Response() readres.success = True readres.data = data writereq = COWrite.Request() writereq.index = 0x4000 writereq.subindex = 0x00 writereq.data = data writeres = COWrite.Response() writeres.success = True self.node.call_service("proxy_device_1/sdo_write", COWrite, writereq, writeres) self.node.call_service("proxy_device_2/sdo_write", COWrite, writereq, writeres) self.node.call_service("proxy_device_1/sdo_read", CORead, readreq, readres) self.node.call_service("proxy_device_2/sdo_read", CORead, readreq, readres) data = 0 self.node.call_service("proxy_device_1/sdo_write", COWrite, writereq, writeres) self.node.call_service("proxy_device_2/sdo_write", COWrite, writereq, writeres) req.transition.id = Transition.TRANSITION_DEACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************DEACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CLEANUP SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CONFIGURE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CONFIGURE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_ACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************ACTIVATE SUCCESSFUL*************************") self.node.call_service("proxy_device_1/sdo_read", CORead, readreq, readres) self.node.call_service("proxy_device_2/sdo_read", CORead, readreq, readres) req.transition.id = Transition.TRANSITION_DEACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************DEACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CLEANUP SUCCESSFUL*************************") class TestPDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_full_cycle_sdo(self): req = ChangeState.Request() res = ChangeState.Response() res.success = True req.transition.id = Transition.TRANSITION_CONFIGURE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CONFIGURE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_ACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************ACTIVATE SUCCESSFUL*************************") msg = COData() msg.index = 0x4000 msg.subindex = 0 msg.data = 200 pub_msg = COData() pub_msg.index = 0x4001 pub_msg.subindex = 0 pub_msg.data = 200 thread = threading.Thread( target=self.node.subscribe_and_wait_for_message, args=["proxy_device_1/rpdo", COData, pub_msg], ) thread.start() self.node.publish_message("proxy_device_1/tpdo", COData, msg) time.sleep(0.1) msg.data = 0 self.node.publish_message("proxy_device_1/tpdo", COData, msg) thread.join() req.transition.id = Transition.TRANSITION_DEACTIVATE self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************DEACTIVATE SUCCESSFUL*************************") req.transition.id = Transition.TRANSITION_CLEANUP self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) print("*************************CLEANUP SUCCESSFUL*************************") ================================================ FILE: canopen_tests/launch_tests/test_robot_control.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from time import sleep import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import threading import rclpy from rclpy.node import Node from canopen_utils.launch_test_node import LaunchTestNode from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID, COTargetDouble from canopen_interfaces.msg import COData from std_srvs.srv import Trigger from std_msgs.msg import Float64MultiArray import unittest @pytest.mark.rostest def generate_test_description(): launch_desc = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), "/robot_control_setup.launch.py", ] ) ) ready_to_test = launch.actions.TimerAction( period=5.0, actions=[launch_testing.actions.ReadyToTest()], ) return (LaunchDescription([launch_desc, ready_to_test]), {}) class TestSDO(unittest.TestCase): def run_node(self): while self.ok: rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod def setUp(cls): cls.ok = True rclpy.init() cls.node = LaunchTestNode() cls.thread = threading.Thread(target=cls.run_node, args=[cls]) cls.thread.start() @classmethod def tearDown(cls): cls.ok = False cls.thread.join() cls.node.destroy_node() rclpy.shutdown() def test_sdo_read(self): req = CORead.Request() req.index = 0x1000 req.subindex = 0 res = CORead.Response() res.success = True res.data = 0x60192 self.node.call_service("joint_1/sdo_read", CORead, req, res) def test_forward_control(self): message = Float64MultiArray() message.data = [1.0, 0.0] self.node.publish_message("forward_position_controller", Float64MultiArray, message) time.sleep(1.0) ================================================ FILE: canopen_tests/package.xml ================================================ canopen_tests 0.3.2 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 ament_cmake lely_core_libraries canopen_402_driver canopen_core canopen_proxy_driver xacro controller_manager robot_state_publisher joint_state_broadcaster joint_trajectory_controller forward_command_controller canopen_fake_slaves canopen_ros2_controllers ament_lint_auto ament_cmake ================================================ FILE: canopen_tests/rviz/robot_controller.rviz ================================================ Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /RobotModel1 Splitter Ratio: 0.5 Tree Height: 752 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false Description File: /home/ws/test.urdf Description Source: Topic Description Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order link1: Alpha: 1 Show Axes: false Show Trail: false Value: true link2: Alpha: 1 Show Axes: false Show Trail: false link3: Alpha: 1 Show Axes: false Show Trail: false link4: Alpha: 1 Show Axes: false Show Trail: false Mass Properties: Inertia: false Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true link1: Value: true link2: Value: true link3: Value: true link4: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: link1: link2: link3: link4: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: link1 Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Covariance x: 0.25 Covariance y: 0.25 Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.785398006439209 Target Frame: Value: Orbit (rviz) Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1043 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d00650100000000000007360000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004c50000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1846 X: 1994 Y: 0 ================================================ FILE: canopen_tests/urdf/canopen_system/canopen_system.ros2_control.xacro ================================================ canopen_ros2_control/CanopenSystem ${bus_config} ${master_config} ${can_interface_name} "${master_bin}" 2 3 ================================================ FILE: canopen_tests/urdf/canopen_system/canopen_system.urdf.xacro ================================================ ================================================ FILE: canopen_tests/urdf/cia402_system/cia402_system.ros2_control.xacro ================================================ canopen_ros2_control/Cia402System ${bus_config} ${master_config} ${can_interface_name} "${master_bin}" 2 ================================================ FILE: canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro ================================================ ================================================ FILE: canopen_tests/urdf/robot_controller/robot_controller.macro.xacro ================================================ ================================================ FILE: canopen_tests/urdf/robot_controller/robot_controller.ros2_control.xacro ================================================ canopen_ros2_control/RobotSystem ${bus_config} ${master_config} ${can_interface_name} "${master_bin}" joint_1 joint_2 ================================================ FILE: canopen_tests/urdf/robot_controller/robot_controller.urdf.xacro ================================================ ================================================ FILE: canopen_utils/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ * Enhance SimpleSlave class with vendor ID parsing and adjust bus configuration timeouts * Contributors: ipa-vsp 0.2.9 (2024-04-16) ------------------ 0.3.1 (2025-06-23) ------------------ 0.3.0 (2024-12-12) ------------------ 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Contributors: ipa-vsp 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ 0.2.5 (2023-06-23) ------------------ 0.2.4 (2023-06-22) ------------------ 0.2.3 (2023-06-22) ------------------ 0.2.2 (2023-06-21) ------------------ 0.2.1 (2023-06-21) ------------------ * Add more tidy launch_test_node.py * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Vishnuprasad Prachandabhanu ================================================ FILE: canopen_utils/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: canopen_utils/canopen_utils/__init__.py ================================================ ================================================ FILE: canopen_utils/canopen_utils/cyclic_tester.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from canopen_interfaces.srv import COTargetDouble class DoubleTalker(Node): """Publish messages to a topic using two publishers at different rates.""" def __init__(self): super().__init__("double_talker") self.i = 0 self.cli = self.create_client(COTargetDouble, "trinamic_pd42/target") while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info("service not available, waiting again...") self.increment = 1000.0 self.period = 0.1 self.value = 0.0 self.group = MutuallyExclusiveCallbackGroup() self.timer = self.create_timer(self.period, self.timer_callback, callback_group=self.group) self.req = COTargetDouble.Request() def timer_callback(self): self.value = self.value + self.increment self.req.target = self.value req = self.cli.call(self.req) if req.success: self.get_logger().info("Success") else: self.get_logger().info("Failure") def main(args=None): rclpy.init(args=args) try: talker = DoubleTalker() # MultiThreadedExecutor executes callbacks with a thread pool. If num_threads is not # specified then num_threads will be multiprocessing.cpu_count() if it is implemented. # Otherwise it will use a single thread. This executor will allow callbacks to happen in # parallel, however the MutuallyExclusiveCallbackGroup in DoubleTalker will only allow its # callbacks to be executed one at a time. The callbacks in Listener are free to execute in # parallel to the ones in DoubleTalker however. executor = MultiThreadedExecutor(num_threads=4) executor.add_node(talker) try: executor.spin() finally: executor.shutdown() talker.destroy_node() finally: rclpy.shutdown() if __name__ == "__main__": main() ================================================ FILE: canopen_utils/canopen_utils/launch_test_node.py ================================================ # Copyright 2023 ROS-Industrial # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import time import re import launch_testing import launch_testing.asserts import launch_testing.actions import launch_testing.util import launch_testing.tools from launch_testing.asserts import assertSequentialStdout from rclpy.node import Node from rclpy.publisher import Publisher from rclpy.subscription import Subscription from rclpy.client import Client from rclpy.qos import QoSProfile from threading import Condition class Ros2ActiveIoHandler: def __init__(self, proc_output: launch_testing.ActiveIoHandler): self.proc_output = proc_output def checkInRos2Stream(self, process, expected_output: str): resolved_procs = launch_testing.util.resolveProcesses( info_obj=self.proc_output, process=process, cmd_args=None, strict_proc_matching=True ) pat = re.compile(r".*\]: " + expected_output) for proc in resolved_procs: for output in self.proc_output[proc]: text = output.text.decode() print("recceived: " + text) res = pat.search(text) if res is not None: return True return False def waitFor(self, process, expected_output: str, timeout: float = 1.0): success = False with self.proc_output._sync_lock: # TODO(pete.baughman): Searching through all of the IO can be time consuming/wasteful. # We can optimize this by making a note of where we left off searching and only # searching new messages when we return from the wait. success = self.proc_output._sync_lock.wait_for( lambda: self.checkInRos2Stream(process=process, expected_output=expected_output), timeout=timeout, ) return success def assertWaitFor(self, process, expected_output: str, timeout: float = 1.0): success = self.waitFor(process=process, expected_output=expected_output, timeout=timeout) assert success, f"Waiting for output {expected_output} timed out" class LaunchTestNode(Node): def __init__(self): super().__init__("launch_test_node") self.get_logger().info("launch_test_node initialized") def publish_delayed(self, publisher: Publisher, msg, delay: float): publisher.publish(msg) time.sleep(delay) def publish_and_check_output( self, publisher: Publisher, msg, expected_output: str, proc_output: launch_testing.ActiveIoHandler, process=None, ): self.publish_delayed(publisher, msg, 0.1) Ros2ActiveIoHandler(proc_output).assertWaitFor( process=process, expected_output=expected_output ) def subscribe_and_wait_for_message(self, topic: str, msg_type, msg, timeout: float = 2.5): condition = Condition() self.received = False def callback(rec_msg): with condition: if not self.received: self.received = msg == rec_msg if self.received: condition.notify() subscription: Subscription = self.create_subscription(msg_type, topic, callback, 1) with condition: condition.wait(timeout=timeout) self.destroy_subscription(subscription) assert self.received, "Did not receive the expected message." def call_service( self, service_name: str, srv_type, service_request, expected_response, timeout: float = 2.0 ): condition = Condition() def callback(arg): with condition: condition.notify() client: Client = self.create_client( srv_type, service_name, qos_profile=QoSProfile(depth=1) ) if not client.wait_for_service(timeout_sec=timeout): assert False, "Service server not available." future = client.call_async(service_request) future.add_done_callback(callback=callback) with condition: if not condition.wait(timeout=timeout): assert False, "Service call timed out." res = future.result() assert res == expected_response, "Did not receive the expected response." self.destroy_client(client) def publish_message(self, topic_name: str, topic_type, msg): publisher = self.create_publisher(topic_type, topic_name, 10) publisher.publish(msg) self.destroy_publisher(publisher) ================================================ FILE: canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from lifecycle_msgs.srv import GetState, ChangeState from lifecycle_msgs.msg import State, Transition from std_srvs.srv import Trigger from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID from canopen_interfaces.msg import COData class SimpleTestNode(Node): def __init__(self, name="test_node"): super().__init__(name) def checkRpdoTpdo(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: publisher = self.create_publisher(COData, "/" + node_name + "/tpdo", 10) subscriber = self.create_subscription( COData, "/" + node_name + "/rpdo", self.rpdo_callback, 10 ) target = COData() target.index = index target.subindex = subindex target.type = type target.data = data publisher.publish(target) print("Published tpdo") # Wait for topic to be published self.rpdo_data = None while not (self.rpdo_data): print("Waiting for subscriber to connect. ") rclpy.spin_once(self, timeout_sec=0.1) if self.rpdo_data.data == data: result = True else: result = False self.destroy_publisher(publisher) self.destroy_subscription(subscriber) return result def rpdo_callback(self, msg): print("RPDO Callback") print(msg) self.rpdo_data = msg def main(args=None): rclpy.init(args=args) node = SimpleTestNode() result = node.checkRpdoTpdo("proxy_device_1", index=0x4000, subindex=0, type=32, data=999) print("RESULT: " + str(result)) result = node.checkRpdoTpdo("proxy_device_2", index=0x4000, subindex=0, type=32, data=999) print("RESULT: " + str(result)) node.destroy_node() rclpy.shutdown() if __name__ == "__main__": main() ================================================ FILE: canopen_utils/canopen_utils/test_node.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import rclpy from rclpy.node import Node from lifecycle_msgs.srv import GetState, ChangeState from lifecycle_msgs.msg import State, Transition from std_srvs.srv import Trigger from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID from canopen_interfaces.msg import COData class TestNode(Node): def __init__(self, name="test_node"): super().__init__(name) def checkTransition(self, node_name: str, state: int, tranisition: int) -> bool: get_state_client = self.create_client(GetState, node_name + "/get_state") change_state_client = self.create_client(ChangeState, node_name + "/change_state") if not get_state_client.wait_for_service(timeout_sec=3.0): get_state_client.destroy() change_state_client.destroy() return False if not change_state_client.wait_for_service(timeout_sec=3.0): get_state_client.destroy() change_state_client.destroy() return False req = GetState.Request() result = get_state_client.call(req) if result.current_state.id != state: return False req = ChangeState.Request() req.transition.id = tranisition result = change_state_client.call(req) if not result.success: get_state_client.destroy() change_state_client.destroy() return False get_state_client.destroy() change_state_client.destroy() return True def checkTrigger(self, node_name, service_name) -> bool: trigger_client = self.create_client(Trigger, "/" + node_name + "/" + service_name) if not trigger_client.wait_for_service(timeout_sec=3.0): return False req = Trigger.Request() result = trigger_client.call(req) trigger_client.destroy() if result.success: return True return False def checkSDORead(self, node_name, index: int, subindex: int, data: int) -> bool: client = self.create_client(CORead, "/" + node_name + "/sdo_read") if not client.wait_for_service(timeout_sec=3.0): return False req = CORead.Request() req.index = index req.subindex = subindex result = client.call(req) client.destroy() if result.success and (data == result.data): return True return False def checkSDOWrite(self, node_name, index: int, subindex: int, data: int) -> bool: client = self.create_client(COWrite, "/" + node_name + "/sdo_write") if not client.wait_for_service(timeout_sec=3.0): return False req = COWrite.Request() req.index = index req.subindex = subindex req.data = data result = client.call(req) client.destroy() if result.success: return True return False def checkSDOReadID( self, node_id: int, index: int, subindex: int, type: int, data: int ) -> bool: client = self.create_client(COReadID, "/master/sdo_read") if not client.wait_for_service(timeout_sec=3.0): return False req = COReadID.Request() req.index = index req.subindex = subindex req.canopen_datatype = type req.nodeid = node_id result = client.call(req) client.destroy() if result.success and (data == result.data): return True return False def checkSDOWriteID( self, node_id: int, index: int, subindex: int, type: int, data: int ) -> bool: client = self.create_client(COWriteID, "/master/sdo_write") if not client.wait_for_service(timeout_sec=3.0): return False req = COWriteID.Request() req.index = index req.subindex = subindex req.data = data req.canopen_datatype = type req.nodeid = node_id result = client.call(req) client.destroy() if result.success: return True return False def checkRpdoTpdo(self, node_name, index: int, subindex: int, data: int) -> bool: publisher = self.create_publisher(COData, "/" + node_name + "/tpdo", 10) subscriber = self.create_subscription( COData, "/" + node_name + "/rpdo", self.rpdo_callback, 10 ) target = COData() target.index = index target.subindex = subindex target.data = data publisher.publish(target) print("Published tpdo to topic: " + "/" + node_name + "/tpdo") self.rpdo_data = None # Wait for topic to be published while not self.rpdo_data: rclpy.spin_once(self, timeout_sec=0.5) self.destroy_publisher(publisher) self.destroy_subscription(subscriber) if self.rpdo_data.data == data: return True return False def rpdo_callback(self, msg): print(msg) self.rpdo_data = msg ================================================ FILE: canopen_utils/no_tests/_test_copyright.py ================================================ # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from ament_copyright.main import main import pytest @pytest.mark.copyright @pytest.mark.linter def test_copyright(): rc = main(argv=[".", "test"]) assert rc == 0, "Found errors" ================================================ FILE: canopen_utils/no_tests/_test_flake8.py ================================================ # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from ament_flake8.main import main_with_errors import pytest @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) ================================================ FILE: canopen_utils/no_tests/_test_pep257.py ================================================ # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from ament_pep257.main import main import pytest @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): rc = main(argv=[".", "test"]) assert rc == 0, "Found code style errors / warnings" ================================================ FILE: canopen_utils/package.xml ================================================ canopen_utils 0.3.2 Utils for working with ros2_canopen. Christoph Hellmann Santos Apache-2.0 rclpy canopen_interfaces lifecycle_msgs std_msgs ament_lint_auto ament_python ================================================ FILE: canopen_utils/resource/canopen_utils ================================================ ================================================ FILE: canopen_utils/setup.cfg ================================================ [develop] script_dir=$base/lib/canopen_utils [install] install_scripts=$base/lib/canopen_utils ================================================ FILE: canopen_utils/setup.py ================================================ # Copyright 2023 ROS-Industrial # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from setuptools import setup package_name = "canopen_utils" setup( name=package_name, version="0.3.2", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), ], install_requires=["setuptools"], zip_safe=True, maintainer="christoph", maintainer_email="christoph.hellmann.santos@ipa.fraunhofer.de", description="TODO: Package description", license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ "cyclic_tester = canopen_utils.cyclic_tester:main", "simple_tester = canopen_utils.simple_rpdo_tpdo_tester:main", ], }, ) ================================================ FILE: lely_core_libraries/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2025-12-05) ------------------ 0.3.1 (2025-06-23) ------------------ * Do not export deprecated Lely IO library (`#318 `_) * Contributors: Gerry Salinas, Patrick Roncagliolo 0.3.0 (2024-12-12) ------------------ * Add yaml dependency to package.xml of `lely_core_libraries` (`#290 `_) 0.2.12 (2024-04-22) ------------------- * 0.2.9 * forthcoming * Merge pull request `#267 `_ from clalancette/clalancette/update-lely-core-hash Update the lely_core_libraries hash to the latest. * Update the lely_core_libraries hash to the latest. This will fix the building of the lely_core_libraries on Ubuntu 24.04. * Contributors: Chris Lalancette, Vishnuprasad Prachandabhanu, ipa-vsp 0.2.9 (2024-04-16) ------------------ * Update the lely_core_libraries hash to the latest. * Contributors: Chris Lalancette, Vishnuprasad Prachandabhanu 0.2.8 (2024-01-19) ------------------ 0.2.7 (2023-06-30) ------------------ * Add missing license headers and activate ament_copyright * Fix python versions * Contributors: Christoph Hellmann Santos 0.2.6 (2023-06-24) ------------------ * Move install from external project to cmake main * Contributors: Christoph Hellmann Santos 0.2.5 (2023-06-23) ------------------ * Directly install lely * Contributors: Christoph Hellmann Santos 0.2.4 (2023-06-22) ------------------ * Add empy dependency for dcfgen * Clean up lely build folder after install for rpm build * Contributors: Christoph Hellmann Santos 0.2.3 (2023-06-22) ------------------ * Fix manual dcfgen python installation on buildfarm * Solve buildfarm dependency issues * Contributors: Christoph Hellmann Santos 0.2.2 (2023-06-21) ------------------ * Reset hard before patch applicaiton * Contributors: Christoph Hellmann Santos 0.2.1 (2023-06-21) ------------------ * Do not build dcf-tools bdist in lely_core_libraries anymore, fixes buildfarm issue * Contributors: Christoph Hellmann Santos 0.2.0 (2023-06-14) ------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos ================================================ FILE: lely_core_libraries/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(lely_core_libraries) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) set(python_version "python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}") include(ExternalProject) ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/upstream INSTALL_DIR "${CMAKE_INSTALL_PREFIX}" # Installation prefix BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/build GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git GIT_TAG fb735b79cab5f0cdda45bc5087414d405ef8f3ab TIMEOUT 60 #UPDATE step apply patch to fix dcf-tools install UPDATE_COMMAND COMMAND git reset --hard COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Fix-dcf-tools.patch #CONFIGURE step execute autoreconf and configure CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag #BUILD STEP execute make BUILD_COMMAND $(MAKE) -C ${CMAKE_CURRENT_BINARY_DIR}/build #INSTALL STEP do nothing as we install in main INSTALL_COMMAND "" ) #INSTALL lely_core_libraries - execute make install install(CODE "execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} install VERBOSE=1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/build)") set(lely_core_cmake_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include("cmake/lely_core_libraries-extras.cmake" NO_POLICY_SCOPE) install( DIRECTORY cmake DESTINATION share/${PROJECT_NAME} ) ament_python_install_package(cogen SCRIPTS_DESTINATION lib/cogen) # install entry-point script(s) in bin as well install( DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_python/cogen/scripts/ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ USE_SOURCE_PERMISSIONS) ament_export_include_directories(include) ament_export_libraries(lely-can lely-co lely-coapp lely-ev lely-io2 lely-libc lely-tap lely-util) ament_package( CONFIG_EXTRAS "cmake/lely_core_libraries-extras.cmake" ) ================================================ FILE: lely_core_libraries/LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2022 Christoph Hellmann Santos Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: lely_core_libraries/cmake/lely_core_libraries-extras.cmake ================================================ # Copyright (c) 2023 Christoph Hellmann Santos # Błażej Sowa # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # macro( generate_dcf TARGET) add_custom_target( ${TARGET}_prepare ALL COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} #COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ ) add_custom_target( ${TARGET} ALL DEPENDS ${TARGET}_prepare ) add_custom_command( TARGET ${TARGET} POST_BUILD COMMAND sed 's|@BUS_CONFIG_PATH@|${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}|g' ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/bus.yml > ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml COMMAND dcfgen -v -d ${CMAKE_BINARY_DIR}/config/${TARGET}/ -rS ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ ) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ PATTERN "bus.yml" EXCLUDE ) install(DIRECTORY ${CMAKE_BINARY_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ ) endmacro() macro( cogen_dcf TARGET) add_custom_target( ${TARGET}_prepare ALL COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} #COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ ) add_custom_target( ${TARGET} ALL DEPENDS ${TARGET}_prepare ) add_custom_command( TARGET ${TARGET} POST_BUILD COMMAND cogen --input-file ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/bus.yml --output-file ${CMAKE_BINARY_DIR}/config/${TARGET}/preprocessed_bus.yml COMMAND sed 's|@BUS_CONFIG_PATH@|${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}|g' ${CMAKE_BINARY_DIR}/config/${TARGET}/preprocessed_bus.yml > ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml COMMAND dcfgen -v -d ${CMAKE_BINARY_DIR}/config/${TARGET}/ -rS ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ ) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ PATTERN "bus.yml" EXCLUDE ) install(DIRECTORY ${CMAKE_BINARY_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ ) endmacro() macro(dcfgen INPUT_DIR FILE OUTPUT_DIR) message(DEPRECATION "dcfgen macro is depreciated and will be remove in beta version. Use generate_dcf instead.") make_directory(${OUTPUT_DIR}) add_custom_target( ${FILE} ALL COMMAND "dcfgen" "-d" ${OUTPUT_DIR} "-rS" ${INPUT_DIR}${FILE} WORKING_DIRECTORY ${INPUT_DIR} ) endmacro() ================================================ FILE: lely_core_libraries/cogen/__init__.py ================================================ ================================================ FILE: lely_core_libraries/cogen/cogen.py ================================================ # Copyright 2022 Christoph Hellmann Santos # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import yaml import argparse from dataclasses import dataclass from dcfgen.cli import Master, Slave def main(): parser = argparse.ArgumentParser( description="Expands the CANopen bus.yml for further processing." ) parser.add_argument("--input-file", type=str, help="The name of the input file", required=True) parser.add_argument( "--output-file", type=str, help="The name of the output file", required=True ) args = parser.parse_args() with open(args.input_file) as input: cfg = yaml.load(input, yaml.FullLoader) master = {} if "master" not in cfg: print("Found no master.") return else: master = cfg["master"] nodes = {} if "nodes" not in cfg: print("Found no nodes entry.") return else: nodes = cfg["nodes"] options = {} if "options" in cfg: options = cfg["options"] defaults = {} if "defaults" in cfg: defaults = cfg["defaults"] # add defaults to each node for node_name in nodes: for entry_name in defaults: nodes[node_name][entry_name] = defaults[entry_name] modified_file = {} modified_file["options"] = options # modified_file["defaults"] = defaults modified_file["master"] = master for node_name in nodes: modified_file[node_name] = nodes[node_name] with open(args.output_file, mode="w") as output: yaml.dump(modified_file, output) return if __name__ == "__main__": main() ================================================ FILE: lely_core_libraries/package.xml ================================================ lely_core_libraries 0.3.2 ROS wrapper for lely-core-libraries Christoph Hellmann Santos Apache-2.0 ament_cmake git autoconf automake libtool python3-empy python3-yaml ament_cmake ================================================ FILE: lely_core_libraries/patches/0001-Fix-dcf-tools.patch ================================================ From 1fcc088cdf64bee181f1141077af01267cc224ad Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Tue, 27 Jun 2023 10:15:06 +0200 Subject: [PATCH] Fix dcf-tools --- configure.ac | 10 +++------- python/dcf-tools/Makefile.am | 23 +++++------------------ 2 files changed, 8 insertions(+), 25 deletions(-) diff --git a/configure.ac b/configure.ac index 1dd9410d..c274636b 100644 --- a/configure.ac +++ b/configure.ac @@ -569,19 +569,15 @@ AC_ARG_ENABLE([python], AS_HELP_STRING([--disable-python], [disable Python tools and bindings])) AM_CONDITIONAL([HAVE_PYTHON2], [false]) -AC_ARG_ENABLE([python2], - AS_HELP_STRING([--disable-python2], [disable Python 2 tools and bindings])) -AS_IF([test "$enable_python" == "no"], [enable_python2=no]) -AS_IF([test "$enable_python2" != "no"], [ - AX_CHECK_PYTHON([2], [AM_CONDITIONAL([HAVE_PYTHON2], [true])]) -]) AM_CONDITIONAL([HAVE_PYTHON3], [false]) AC_ARG_ENABLE([python3], AS_HELP_STRING([--disable-python3], [disable Python 3 tools and bindings])) AS_IF([test "$enable_python" == "no"], [enable_python3=no]) AS_IF([test "$enable_python3" != "no"], [ - AX_CHECK_PYTHON([3], [AM_CONDITIONAL([HAVE_PYTHON3], [true])]) + AM_PATH_PYTHON([3.3],, [:]) + AC_SUBST(PYTHON3, "$PYTHON") + AM_CONDITIONAL([HAVE_PYTHON3], [test "$PYTHON" != :]) ]) AM_CONDITIONAL([NO_CYTHON], [false]) diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am index 9852c84a..dd5cdfa0 100644 --- a/python/dcf-tools/Makefile.am +++ b/python/dcf-tools/Makefile.am @@ -23,30 +23,17 @@ EXTRA_DIST += setup.py build_base = $(realpath $(builddir))/build dist_dir = $(realpath $(builddir))/dist -all-local: python-sdist python-bdist_wheel - clean-local: rm -rf $(build_base) $(dist_dir) $(srcdir)/*.egg-info $(builddir)/*.egg-info install-exec-local: python-install -.PHONY: python-bdist_wheel -python-bdist_wheel: -if HAVE_PYTHON3 - @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ - bdist_wheel --dist-dir $(dist_dir) -endif - .PHONY: python-install python-install: if HAVE_PYTHON3 - @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ - install --prefix $(DESTDIR)$(prefix) --root / -endif - -.PHONY: python-sdist -python-sdist: -if HAVE_PYTHON3 - @cd $(srcdir); $(PYTHON3_ENV) $(PYTHON3) setup.py \ - sdist --dist-dir $(dist_dir) + @cd $(srcdir) && $(PYTHON3_ENV) $(PYTHON3) setup.py \ + egg_info build install --record install.log \ + --install-scripts $(DESTDIR)$(prefix)/bin/ \ + --install-lib $(DESTDIR)$(prefix)/lib/python$(PYTHON_VERSION)/site-packages/ \ + --single-version-externally-managed endif -- 2.34.1 ================================================ FILE: lely_core_libraries/setup.cfg ================================================ [options.entry_points] console_scripts = cogen = cogen.cogen:main