├── .ci └── alarm_subcode_doc_linter.py ├── .editorconfig ├── .gitattributes ├── .github ├── dependabot.yml ├── matchers │ ├── gcc.json │ └── mpbuilder.json └── workflows │ ├── ci_check_subcode_docs.yaml │ ├── ci_md_lint.yaml │ ├── ci_msbuild.yaml │ ├── ci_reuse_lint.yaml │ └── ci_yamllint.yaml ├── .gitignore ├── .markdownlint.yaml ├── .reuse └── dep5 ├── CHANGELOG.md ├── COLCON_IGNORE ├── CONTRIBUTING.md ├── LICENSES ├── Apache-2.0.txt ├── BSD-2-Clause.txt ├── BSD-3-Clause.txt ├── CC-BY-NC-ND-4.0.txt ├── CC-BY-SA-4.0.txt ├── CC0-1.0.txt ├── LicenseRef-Microsoft-AllowedUses-Screenshots.txt └── MIT.txt ├── MotoROS2.sln ├── README.md ├── config ├── .gitattributes ├── .yamllint └── motoros2_config.yaml ├── doc ├── build_from_source.md ├── faq.md ├── img │ ├── add_library.png │ ├── configuration_manager.png │ ├── intellisense_search_path.png │ ├── logo.png │ ├── properties_configuration.png │ └── vc_directories.png ├── network_configuration.md ├── ros_api.md └── troubleshooting.md ├── lib ├── CmosParameterExtraction.h ├── CmosParameterTypes.h ├── MotoPlusExterns.h ├── MotoROS_PlatformLib.dnLib ├── MotoROS_PlatformLib.h ├── MotoROS_PlatformLib.yrcLib ├── MotoROS_PlatformLib.yrcmLib ├── ParameterExtraction.dnLib ├── ParameterExtraction.yrcLib └── ParameterExtraction.yrcmLib ├── robot_jobs ├── .gitattributes ├── README.md ├── sda_dual_arm │ ├── INIT_ROS.JBI │ └── IONAME.DAT ├── single_arm │ ├── INIT_ROS.JBI │ └── IONAME.DAT ├── single_arm_with_base_axis │ ├── INIT_ROS.JBI │ └── IONAME.DAT ├── single_arm_with_ext_axis │ ├── INIT_ROS.JBI │ └── IONAME.DAT └── two_arms │ ├── INIT_ROS.JBI │ └── IONAME.DAT ├── src ├── ActionServer_FJT.c ├── ActionServer_FJT.h ├── CommunicationExecutor.c ├── CommunicationExecutor.h ├── ConfigFile.c ├── ConfigFile.h ├── ControllerStatusIO.c ├── ControllerStatusIO.h ├── CtrlGroup.c ├── CtrlGroup.h ├── DX200_foxyCompilerArguments.mps ├── DX200_foxyLinkerArguments.mps ├── DX200_galacticCompilerArguments.mps ├── DX200_galacticLinkerArguments.mps ├── DX200_humbleCompilerArguments.mps ├── DX200_humbleLinkerArguments.mps ├── DX200_jazzyCompilerArguments.mps ├── DX200_jazzyLinkerArguments.mps ├── Debug.c ├── Debug.h ├── ErrorHandling.c ├── ErrorHandling.h ├── FauxCommandLineArgs.c ├── FauxCommandLineArgs.h ├── FileUtilityFunctions.c ├── FileUtilityFunctions.h ├── InformCheckerAndGenerator.c ├── InformCheckerAndGenerator.h ├── MathConstants.h ├── MemoryAllocation.c ├── MemoryAllocation.h ├── MemoryTracing.h ├── MotionControl.c ├── MotionControl.h ├── MotoROS.h ├── MotoROS2_AllControllers.vcxproj ├── MotoROS2_AllControllers.vcxproj.filters ├── PositionMonitor.c ├── PositionMonitor.h ├── Quaternion_Conversion.c ├── Quaternion_Conversion.h ├── RosApiNameConstants.h ├── RosMotoPlusConversionUtils.c ├── RosMotoPlusConversionUtils.h ├── Ros_mpGetRobotCalibrationData.c ├── Ros_mpGetRobotCalibrationData.h ├── ServiceQueueTrajPoint.c ├── ServiceQueueTrajPoint.h ├── ServiceReadWriteIO.c ├── ServiceReadWriteIO.h ├── ServiceResetError.c ├── ServiceResetError.h ├── ServiceSelectMotionTool.c ├── ServiceSelectMotionTool.h ├── ServiceStartPointQueueMode.c ├── ServiceStartPointQueueMode.h ├── ServiceStartTrajMode.c ├── ServiceStartTrajMode.h ├── ServiceStopTrajMode.c ├── ServiceStopTrajMode.h ├── Tests_ActionServer_FJT.c ├── Tests_ActionServer_FJT.h ├── Tests_ControllerStatusIO.c ├── Tests_ControllerStatusIO.h ├── Tests_CtrlGroup.c ├── Tests_CtrlGroup.h ├── Tests_RosMotoPlusConversionUtils.c ├── Tests_RosMotoPlusConversionUtils.h ├── Tests_TestUtils.c ├── Tests_TestUtils.h ├── Tests_TimeConversionUtils.c ├── Tests_TimeConversionUtils.h ├── TimeConversionUtils.h ├── YRC1000_foxyCompilerArguments.mps ├── YRC1000_foxyLinkerArguments.mps ├── YRC1000_galacticCompilerArguments.mps ├── YRC1000_galacticLinkerArguments.mps ├── YRC1000_humbleCompilerArguments.mps ├── YRC1000_humbleLinkerArguments.mps ├── YRC1000_jazzyCompilerArguments.mps ├── YRC1000_jazzyLinkerArguments.mps ├── YRC1000u_foxyCompilerArguments.mps ├── YRC1000u_foxyLinkerArguments.mps ├── YRC1000u_galacticCompilerArguments.mps ├── YRC1000u_galacticLinkerArguments.mps ├── YRC1000u_humbleCompilerArguments.mps ├── YRC1000u_humbleLinkerArguments.mps ├── YRC1000u_jazzyCompilerArguments.mps ├── YRC1000u_jazzyLinkerArguments.mps ├── controllers.xml ├── image_comments │ ├── flange_vs_tool0.png │ └── tf_diagram.png └── main.c └── tools ├── .gitignore ├── debug_listener.cmd └── debug_listener.py /.editorconfig: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | # https://editorconfig.org 7 | # https://learn.microsoft.com/en-us/visualstudio/ide/create-portable-custom-editor-options 8 | 9 | root = true 10 | 11 | [*] 12 | insert_final_newline = true 13 | 14 | [*.{c,h}] 15 | indent_size = 4 16 | indent_style = space 17 | 18 | [config/*.yaml] 19 | end_of_line = crlf 20 | indent_size = 2 21 | indent_style = space 22 | 23 | [tools/*.py] 24 | indent_size = 4 25 | indent_style = space 26 | 27 | [*.{DAT,JBI}] 28 | # force Windows EOLs, to prevent controller errors 29 | end_of_line = crlf 30 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | ############################################################################### 7 | # Set default behavior to automatically normalize line endings. 8 | ############################################################################### 9 | * text=auto 10 | 11 | ############################################################################### 12 | # Set default behavior for command prompt diff. 13 | # 14 | # This is need for earlier builds of msysgit that does not have it on by 15 | # default for csharp files. 16 | # Note: This is only used by command line 17 | ############################################################################### 18 | #*.cs diff=csharp 19 | 20 | ############################################################################### 21 | # Set the merge driver for project and solution files 22 | # 23 | # Merging from the command prompt will add diff markers to the files if there 24 | # are conflicts (Merging from VS is not affected by the settings below, in VS 25 | # the diff markers are never inserted). Diff markers may cause the following 26 | # file extensions to fail to load in VS. An alternative would be to treat 27 | # these files as binary and thus will always conflict and require user 28 | # intervention with every merge. To do so, just uncomment the entries below 29 | ############################################################################### 30 | #*.sln merge=binary 31 | #*.csproj merge=binary 32 | #*.vbproj merge=binary 33 | #*.vcxproj merge=binary 34 | #*.vcproj merge=binary 35 | #*.dbproj merge=binary 36 | #*.fsproj merge=binary 37 | #*.lsproj merge=binary 38 | #*.wixproj merge=binary 39 | #*.modelproj merge=binary 40 | #*.sqlproj merge=binary 41 | #*.wwaproj merge=binary 42 | 43 | ############################################################################### 44 | # behavior for image files 45 | # 46 | # image files are treated as binary by default. 47 | ############################################################################### 48 | #*.jpg binary 49 | #*.png binary 50 | #*.gif binary 51 | 52 | ############################################################################### 53 | # diff behavior for common document formats 54 | # 55 | # Convert binary document formats to text before diffing them. This feature 56 | # is only available from the command line. Turn it on by uncommenting the 57 | # entries below. 58 | ############################################################################### 59 | #*.doc diff=astextplain 60 | #*.DOC diff=astextplain 61 | #*.docx diff=astextplain 62 | #*.DOCX diff=astextplain 63 | #*.dot diff=astextplain 64 | #*.DOT diff=astextplain 65 | #*.pdf diff=astextplain 66 | #*.PDF diff=astextplain 67 | #*.rtf diff=astextplain 68 | #*.RTF diff=astextplain 69 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | # Update actions in workflows weekly 7 | version: 2 8 | updates: 9 | - package-ecosystem: "github-actions" 10 | directory: "/" 11 | schedule: 12 | interval: "weekly" 13 | -------------------------------------------------------------------------------- /.github/matchers/gcc.json: -------------------------------------------------------------------------------- 1 | { 2 | "problemMatcher": [ 3 | { 4 | "owner": "gcc", 5 | "pattern": [ 6 | { 7 | "regexp": "^(.*?):(\\d+):(\\d*):?\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$", 8 | "file": 1, 9 | "line": 2, 10 | "column": 3, 11 | "severity": 4, 12 | "message": 5 13 | } 14 | ] 15 | } 16 | ] 17 | } 18 | -------------------------------------------------------------------------------- /.github/matchers/mpbuilder.json: -------------------------------------------------------------------------------- 1 | { 2 | "problemMatcher": [ 3 | { 4 | "owner": "mpBuilder", 5 | "pattern": [ 6 | { 7 | "regexp": "(.*)\\((\\d+)\\):\\s+(\\w+)\\s+:\\s+(.*)\\s+\\[\\S+\\]$", 8 | "file": 1, 9 | "line": 2, 10 | "severity": 3, 11 | "message": 4 12 | } 13 | ] 14 | } 15 | ] 16 | } 17 | -------------------------------------------------------------------------------- /.github/workflows/ci_check_subcode_docs.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | name: "CI: lint alarm subcode docs" 7 | on: 8 | pull_request: 9 | workflow_dispatch: 10 | jobs: 11 | alarm_lint: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v4 15 | - name: Set up Python (3.8) 16 | uses: actions/setup-python@v5 17 | with: 18 | python-version: '3.8' 19 | - name: Install dependencies 20 | run: | 21 | python -m pip install --upgrade pip 22 | python -m pip install mistletoe==1.2.1 pycparser==2.21 23 | - name: Register GCC problem matcher 24 | run: echo "::add-matcher::.github/matchers/gcc.json" 25 | - name: Check alarm subcode documentation 26 | run: | 27 | python \ 28 | .ci/alarm_subcode_doc_linter.py \ 29 | --check-all \ 30 | --ignore-enum Failed_Trajectory_Status \ 31 | --ignore-enum Init_Trajectory_Status \ 32 | --ignore-enum MotionNotReadyCode \ 33 | src/ErrorHandling.h \ 34 | doc/troubleshooting.md 35 | -------------------------------------------------------------------------------- /.github/workflows/ci_md_lint.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | name: "CI: lint Markdown" 7 | on: 8 | pull_request: 9 | workflow_dispatch: 10 | jobs: 11 | md_lint: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v4 15 | - uses: DavidAnson/markdownlint-cli2-action@v19 16 | with: 17 | globs: "**/*.md" 18 | -------------------------------------------------------------------------------- /.github/workflows/ci_msbuild.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | name: "CI: build VS proj" 7 | on: 8 | # MSBuild CI only runs for PRs originating from this repository (due to 9 | # access to secrets). Enable CI runs on pushes to the 'main' branch, to 10 | # make sure MSBuild CI also gets run for contributions from external 11 | # repositories (after the PR has been merged, but it might still catch 12 | # problems that slipped past reviewers) 13 | push: 14 | branches: 15 | - main 16 | pull_request: 17 | branches: 18 | - main 19 | workflow_dispatch: 20 | 21 | jobs: 22 | msbuild_filter: 23 | runs-on: ubuntu-latest 24 | outputs: 25 | needs_msbuild: ${{ steps.paths_filter.outputs.vsproj }} 26 | steps: 27 | - name: Checkout MotoROS2 28 | uses: actions/checkout@v4 29 | - name: Check VS proj has changed 30 | id: paths_filter 31 | uses: dorny/paths-filter@v3 32 | with: 33 | filters: | 34 | vsproj: 35 | - 'src/**' 36 | - 'lib/**' 37 | - 'MotoROS2.sln' 38 | 39 | msbuild: 40 | runs-on: windows-2022 41 | needs: msbuild_filter 42 | # only run this job if: 43 | # - this is a manually triggered build, or 44 | # - it's a PR and: 45 | # - source files were changed, 46 | # - AND the PR came from a feature branch on the main repository 47 | if: | 48 | github.event_name == 'workflow_dispatch' || 49 | ( 50 | needs.msbuild_filter.outputs.needs_msbuild == 'true' && 51 | github.event.pull_request.head.repo.full_name == github.repository 52 | ) 53 | 54 | strategy: 55 | # keep jobs running even if one fails (we want to know on which 56 | # controllers and for which ROS 2 versions builds fail / succeed) 57 | fail-fast: false 58 | matrix: 59 | controller: [dx200, yrc1000, yrc1000u] 60 | # officially matrix doesn't support non-scalar values, but it does 61 | # seem to work, so let's use it. 62 | uros: [ 63 | { release: 20250207, codename: foxy }, 64 | { release: 20250207, codename: galactic }, 65 | { release: 20250328, codename: humble }, 66 | { release: 20250328, codename: jazzy }, 67 | ] 68 | 69 | steps: 70 | - name: Uppercase controller identifier 71 | id: uppercaser 72 | shell: bash 73 | env: 74 | TEMP_VAR: "${{ matrix.controller }}" 75 | run: | 76 | echo "ctrlr=${TEMP_VAR^^}" >> $GITHUB_OUTPUT 77 | 78 | - name: Checkout MotoROS2 79 | uses: actions/checkout@v4 80 | with: 81 | path: 'motoros2' 82 | 83 | - name: Find MSBuild and add to PATH 84 | uses: microsoft/setup-msbuild@v2 85 | 86 | - name: Generate token (micro_ros_motoplus) 87 | id: gen_token 88 | uses: actions/create-github-app-token@v1 89 | with: 90 | app-id: ${{ secrets.MPLMB_APP_ID }} 91 | private-key: ${{ secrets.MPLMB_APP_PRIVATE_KEY }} 92 | owner: "yaskawa-global" 93 | repositories: "micro_ros_motoplus" 94 | 95 | - name: "Download M+ libmicroros (${{ matrix.uros.codename }}-${{ matrix.uros.release }} for ${{ steps.uppercaser.outputs.ctrlr }})" 96 | shell: bash 97 | env: 98 | GITHUB_TOKEN: ${{ steps.gen_token.outputs.token }} 99 | RELEASE_TYPE: ${{ (matrix.uros.prerelease && 'prerelease') || 'release' }} 100 | run: | 101 | gh \ 102 | release \ 103 | --repo="yaskawa-global/micro_ros_motoplus" \ 104 | download \ 105 | --pattern="micro_ros_motoplus*.zip" \ 106 | --output="micro_ros_motoplus.zip" \ 107 | ${{ env.RELEASE_TYPE }}-${{ matrix.uros.codename }}-${{ matrix.controller }}-${{ matrix.uros.release }} 108 | 109 | - name: Setup MotoROS2 build dir 110 | shell: bash 111 | run: | 112 | mkdir -p /c/build 113 | rm -rf /c/build/* 114 | # we do this to shorten the path. Windows/M+ SDK sometimes does 115 | cp -r "${{ github.workspace }}"/* /c/build/ 116 | ls -al /c/build 117 | 118 | - name: Extract M+ libmicroros distribution 119 | shell: bash 120 | run: | 121 | unzip -q /c/build/micro_ros_motoplus.zip \ 122 | -d /c/build/motoros2/libmicroros_${{ matrix.controller }}_${{ matrix.uros.codename }} 123 | 124 | - name: Download and setup M+ SDK (mpBuilder) 125 | shell: cmd 126 | run: | 127 | C:\msys64\usr\bin\wget.exe -q -O C:/build/mpsdk_ci.7z "${{ secrets.MPSDK_DOWNLOAD_URL }}" 128 | "C:\Program Files\7-Zip\7z.exe" x C:/build/mpsdk_ci.7z -oC:/build/ 129 | del /q C:\build\mpsdk_ci.7z 130 | - name: Add M+ SDK to PATH 131 | shell: bash 132 | run: echo "C:/build/mpsdk" >> $GITHUB_PATH 133 | 134 | - name: "Build MotoROS2 (config: ${{ steps.uppercaser.outputs.ctrlr }}_${{ matrix.uros.codename }})" 135 | shell: cmd 136 | env: 137 | MP_VS_Install: "C:/build/mpsdk/" 138 | WIND_BASE: "C:/" 139 | WIND_HOST_TYPE: "x86-win32" 140 | WIND_USR: "C:/" 141 | run: | 142 | msbuild ^ 143 | C:\build\motoros2\MotoROS2.sln ^ 144 | -t:Build ^ 145 | -nologo ^ 146 | -v:minimal ^ 147 | -p:Configuration=${{ steps.uppercaser.outputs.ctrlr }}_${{ matrix.uros.codename }} 148 | -------------------------------------------------------------------------------- /.github/workflows/ci_reuse_lint.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | name: "CI: check REUSE compliance" 7 | on: 8 | pull_request: 9 | workflow_dispatch: 10 | jobs: 11 | reuse_lint: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v4 15 | - uses: fsfe/reuse-action@v5 16 | -------------------------------------------------------------------------------- /.github/workflows/ci_yamllint.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | name: "CI: lint MotoROS2 config" 7 | on: 8 | pull_request: 9 | workflow_dispatch: 10 | jobs: 11 | yaml_lint: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v4 15 | - uses: karancode/yamllint-github-action@v3.0.0 16 | with: 17 | yamllint_file_or_dir: 'config' 18 | yamllint_config_filepath: 'config/.yamllint' 19 | yamllint_strict: true 20 | yamllint_comment: true 21 | env: 22 | GITHUB_ACCESS_TOKEN: ${{ secrets.GITHUB_TOKEN }} 23 | -------------------------------------------------------------------------------- /.markdownlint.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | # Default state for all rules 7 | default: true 8 | 9 | # MD013/line-length - Line length 10 | # we disable this because we use semantic line breaks (one-sentence-per-line, 11 | # specifically) so we can't put a maximum length on lines. 12 | MD013: false 13 | 14 | # MD033 - Inline HTML 15 | # as we use an 'img' tag to show the logo at the top of the readme, we 16 | # explicitly allow it here 17 | MD033: 18 | allowed_elements: ['h1', 'img'] 19 | 20 | # MD050 - Strong style 21 | # we use the double asterisk style throughout our documentation 22 | MD050: 23 | style: asterisk 24 | -------------------------------------------------------------------------------- /.reuse/dep5: -------------------------------------------------------------------------------- 1 | Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ 2 | Upstream-Name: motoros2 3 | Upstream-Contact: Ted Miller 4 | G. vd. Hoorn 5 | Source: https://github.com/yaskawa-global/motoros2 6 | 7 | Files: src/image_comments/* 8 | Copyright: 2022-2023 Yaskawa America, Inc. 9 | License: CC-BY-SA-4.0 10 | 11 | Files: robot_jobs/*.JBI 12 | Copyright: 2022-2023 Yaskawa America, Inc. 13 | License: Apache-2.0 14 | 15 | Files: robot_jobs/*.DAT 16 | Copyright: 2022-2023 Yaskawa America, Inc. 17 | License: Apache-2.0 18 | 19 | Files: src/*.mps 20 | Copyright: 2022-2023 Yaskawa America, Inc. 21 | License: Apache-2.0 22 | 23 | Files: lib/ParameterExtraction.* 24 | Copyright: 2013-2023 Yaskawa America, Inc. 25 | License: BSD-3-Clause 26 | 27 | Files: lib/MotoROS_PlatformLib.*Lib 28 | Copyright: 2023 Yaskawa America, Inc. 29 | 2023, Delft University of Technology 30 | License: Apache-2.0 31 | 32 | Files: src/*.vcxproj src/*.vcxproj.filters *.sln 33 | Copyright: 2022-2023 Yaskawa America, Inc. 34 | License: CC0-1.0 35 | 36 | # screenshots in documentation 37 | Files: doc/img/add_library.png doc/img/configuration_manager.png doc/img/intellisense_search_path.png doc/img/properties_configuration.png doc/img/vc_directories.png 38 | Copyright: 2023 Microsoft 39 | License: LicenseRef-Microsoft-AllowedUses-Screenshots 40 | 41 | Files: .github/matchers/mpbuilder.json .github/matchers/gcc.json 42 | Copyright: 2023 Yaskawa America, Inc. 43 | 2023, Delft University of Technology 44 | License: CC0-1.0 45 | 46 | Files: doc/img/logo.png 47 | Copyright: 2023 Yaskawa America, Inc. 48 | License: CC-BY-NC-ND-4.0 49 | -------------------------------------------------------------------------------- /COLCON_IGNORE: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | MotoROS2 is not a ROS 2 package. 7 | 8 | It can not be built using Colcon. 9 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | 7 | 8 | # Contributing 9 | 10 | All contributions made to this repository will be under the `Apache-2.0` License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): 11 | 12 | ```text 13 | 5. Submission of Contributions. Unless You explicitly state otherwise, 14 | any Contribution intentionally submitted for inclusion in the Work 15 | by You to the Licensor shall be under the terms and conditions of 16 | this License, without any additional terms or conditions. 17 | Notwithstanding the above, nothing herein shall supersede or modify 18 | the terms of any separate license agreement you may have executed 19 | with Licensor regarding such Contributions. 20 | ``` 21 | -------------------------------------------------------------------------------- /LICENSES/BSD-2-Clause.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. 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. 8 | 9 | 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. 10 | -------------------------------------------------------------------------------- /LICENSES/BSD-3-Clause.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) . 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. 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. 8 | 9 | 3. 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. 10 | 11 | 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. 12 | -------------------------------------------------------------------------------- /LICENSES/CC0-1.0.txt: -------------------------------------------------------------------------------- 1 | Creative Commons Legal Code 2 | 3 | CC0 1.0 Universal 4 | 5 | CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE 6 | LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN 7 | ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS 8 | INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES 9 | REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS 10 | PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM 11 | THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED 12 | HEREUNDER. 13 | 14 | Statement of Purpose 15 | 16 | The laws of most jurisdictions throughout the world automatically confer 17 | exclusive Copyright and Related Rights (defined below) upon the creator 18 | and subsequent owner(s) (each and all, an "owner") of an original work of 19 | authorship and/or a database (each, a "Work"). 20 | 21 | Certain owners wish to permanently relinquish those rights to a Work for 22 | the purpose of contributing to a commons of creative, cultural and 23 | scientific works ("Commons") that the public can reliably and without fear 24 | of later claims of infringement build upon, modify, incorporate in other 25 | works, reuse and redistribute as freely as possible in any form whatsoever 26 | and for any purposes, including without limitation commercial purposes. 27 | These owners may contribute to the Commons to promote the ideal of a free 28 | culture and the further production of creative, cultural and scientific 29 | works, or to gain reputation or greater distribution for their Work in 30 | part through the use and efforts of others. 31 | 32 | For these and/or other purposes and motivations, and without any 33 | expectation of additional consideration or compensation, the person 34 | associating CC0 with a Work (the "Affirmer"), to the extent that he or she 35 | is an owner of Copyright and Related Rights in the Work, voluntarily 36 | elects to apply CC0 to the Work and publicly distribute the Work under its 37 | terms, with knowledge of his or her Copyright and Related Rights in the 38 | Work and the meaning and intended legal effect of CC0 on those rights. 39 | 40 | 1. Copyright and Related Rights. A Work made available under CC0 may be 41 | protected by copyright and related or neighboring rights ("Copyright and 42 | Related Rights"). Copyright and Related Rights include, but are not 43 | limited to, the following: 44 | 45 | i. the right to reproduce, adapt, distribute, perform, display, 46 | communicate, and translate a Work; 47 | ii. moral rights retained by the original author(s) and/or performer(s); 48 | iii. publicity and privacy rights pertaining to a person's image or 49 | likeness depicted in a Work; 50 | iv. rights protecting against unfair competition in regards to a Work, 51 | subject to the limitations in paragraph 4(a), below; 52 | v. rights protecting the extraction, dissemination, use and reuse of data 53 | in a Work; 54 | vi. database rights (such as those arising under Directive 96/9/EC of the 55 | European Parliament and of the Council of 11 March 1996 on the legal 56 | protection of databases, and under any national implementation 57 | thereof, including any amended or successor version of such 58 | directive); and 59 | vii. other similar, equivalent or corresponding rights throughout the 60 | world based on applicable law or treaty, and any national 61 | implementations thereof. 62 | 63 | 2. Waiver. To the greatest extent permitted by, but not in contravention 64 | of, applicable law, Affirmer hereby overtly, fully, permanently, 65 | irrevocably and unconditionally waives, abandons, and surrenders all of 66 | Affirmer's Copyright and Related Rights and associated claims and causes 67 | of action, whether now known or unknown (including existing as well as 68 | future claims and causes of action), in the Work (i) in all territories 69 | worldwide, (ii) for the maximum duration provided by applicable law or 70 | treaty (including future time extensions), (iii) in any current or future 71 | medium and for any number of copies, and (iv) for any purpose whatsoever, 72 | including without limitation commercial, advertising or promotional 73 | purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each 74 | member of the public at large and to the detriment of Affirmer's heirs and 75 | successors, fully intending that such Waiver shall not be subject to 76 | revocation, rescission, cancellation, termination, or any other legal or 77 | equitable action to disrupt the quiet enjoyment of the Work by the public 78 | as contemplated by Affirmer's express Statement of Purpose. 79 | 80 | 3. Public License Fallback. Should any part of the Waiver for any reason 81 | be judged legally invalid or ineffective under applicable law, then the 82 | Waiver shall be preserved to the maximum extent permitted taking into 83 | account Affirmer's express Statement of Purpose. In addition, to the 84 | extent the Waiver is so judged Affirmer hereby grants to each affected 85 | person a royalty-free, non transferable, non sublicensable, non exclusive, 86 | irrevocable and unconditional license to exercise Affirmer's Copyright and 87 | Related Rights in the Work (i) in all territories worldwide, (ii) for the 88 | maximum duration provided by applicable law or treaty (including future 89 | time extensions), (iii) in any current or future medium and for any number 90 | of copies, and (iv) for any purpose whatsoever, including without 91 | limitation commercial, advertising or promotional purposes (the 92 | "License"). The License shall be deemed effective as of the date CC0 was 93 | applied by Affirmer to the Work. Should any part of the License for any 94 | reason be judged legally invalid or ineffective under applicable law, such 95 | partial invalidity or ineffectiveness shall not invalidate the remainder 96 | of the License, and in such case Affirmer hereby affirms that he or she 97 | will not (i) exercise any of his or her remaining Copyright and Related 98 | Rights in the Work or (ii) assert any associated claims and causes of 99 | action with respect to the Work, in either case contrary to Affirmer's 100 | express Statement of Purpose. 101 | 102 | 4. Limitations and Disclaimers. 103 | 104 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 105 | surrendered, licensed or otherwise affected by this document. 106 | b. Affirmer offers the Work as-is and makes no representations or 107 | warranties of any kind concerning the Work, express, implied, 108 | statutory or otherwise, including without limitation warranties of 109 | title, merchantability, fitness for a particular purpose, non 110 | infringement, or the absence of latent or other defects, accuracy, or 111 | the present or absence of errors, whether or not discoverable, all to 112 | the greatest extent permissible under applicable law. 113 | c. Affirmer disclaims responsibility for clearing rights of other persons 114 | that may apply to the Work or any use thereof, including without 115 | limitation any person's Copyright and Related Rights in the Work. 116 | Further, Affirmer disclaims responsibility for obtaining any necessary 117 | consents, permissions or other rights required for any use of the 118 | Work. 119 | d. Affirmer understands and acknowledges that Creative Commons is not a 120 | party to this document and has no duty or obligation with respect to 121 | this CC0 or use of the Work. 122 | -------------------------------------------------------------------------------- /LICENSES/LicenseRef-Microsoft-AllowedUses-Screenshots.txt: -------------------------------------------------------------------------------- 1 | Screenshots of Microsoft Visual Studio used with permission from Microsoft in accordance with [1] and [2]. 2 | 3 | [1] https://www.microsoft.com/en-us/legal/intellectualproperty/copyright/permissions, online, retrieved 2023-05-11 4 | [2] https://www.microsoft.com/en-us/legal/intellectualproperty/copyright/permissions#Screen%20Shots, online, retrieved 2023-05-11 5 | -------------------------------------------------------------------------------- /LICENSES/MIT.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 4 | 5 | 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: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | 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. 10 | -------------------------------------------------------------------------------- /MotoROS2.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio Version 17 4 | VisualStudioVersion = 17.3.32901.215 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MotoROS2", "src\MotoROS2_AllControllers.vcxproj", "{64C31524-A3C4-49D4-AD04-955D44202226}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | DX200_foxy|x86 = DX200_foxy|x86 11 | DX200_galactic|x86 = DX200_galactic|x86 12 | DX200_humble|x86 = DX200_humble|x86 13 | DX200_jazzy|x86 = DX200_jazzy|x86 14 | YRC1000_foxy|x86 = YRC1000_foxy|x86 15 | YRC1000_galactic|x86 = YRC1000_galactic|x86 16 | YRC1000_humble|x86 = YRC1000_humble|x86 17 | YRC1000_jazzy|x86 = YRC1000_jazzy|x86 18 | YRC1000u_foxy|x86 = YRC1000u_foxy|x86 19 | YRC1000u_galactic|x86 = YRC1000u_galactic|x86 20 | YRC1000u_humble|x86 = YRC1000u_humble|x86 21 | YRC1000u_jazzy|x86 = YRC1000u_jazzy|x86 22 | EndGlobalSection 23 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 24 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_foxy|x86.ActiveCfg = DX200_foxy|Win32 25 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_foxy|x86.Build.0 = DX200_foxy|Win32 26 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_galactic|x86.ActiveCfg = DX200_galactic|Win32 27 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_galactic|x86.Build.0 = DX200_galactic|Win32 28 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_humble|x86.ActiveCfg = DX200_humble|Win32 29 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_humble|x86.Build.0 = DX200_humble|Win32 30 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_jazzy|x86.ActiveCfg = DX200_jazzy|Win32 31 | {64C31524-A3C4-49D4-AD04-955D44202226}.DX200_jazzy|x86.Build.0 = DX200_jazzy|Win32 32 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_foxy|x86.ActiveCfg = YRC1000_foxy|Win32 33 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_foxy|x86.Build.0 = YRC1000_foxy|Win32 34 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_galactic|x86.ActiveCfg = YRC1000_galactic|Win32 35 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_galactic|x86.Build.0 = YRC1000_galactic|Win32 36 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_humble|x86.ActiveCfg = YRC1000_humble|Win32 37 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_humble|x86.Build.0 = YRC1000_humble|Win32 38 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_jazzy|x86.ActiveCfg = YRC1000_jazzy|Win32 39 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000_jazzy|x86.Build.0 = YRC1000_jazzy|Win32 40 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_foxy|x86.ActiveCfg = YRC1000u_foxy|Win32 41 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_foxy|x86.Build.0 = YRC1000u_foxy|Win32 42 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_galactic|x86.ActiveCfg = YRC1000u_galactic|Win32 43 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_galactic|x86.Build.0 = YRC1000u_galactic|Win32 44 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_humble|x86.ActiveCfg = YRC1000u_humble|Win32 45 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_humble|x86.Build.0 = YRC1000u_humble|Win32 46 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_jazzy|x86.ActiveCfg = YRC1000u_jazzy|Win32 47 | {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u_jazzy|x86.Build.0 = YRC1000u_jazzy|Win32 48 | EndGlobalSection 49 | GlobalSection(SolutionProperties) = preSolution 50 | HideSolutionNode = FALSE 51 | EndGlobalSection 52 | GlobalSection(ExtensibilityGlobals) = postSolution 53 | SolutionGuid = {C6611CF4-3743-47F8-BC5B-D98E9D8FD2C0} 54 | EndGlobalSection 55 | EndGlobal 56 | -------------------------------------------------------------------------------- /config/.gitattributes: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | # we force Windows-style line-endings in the configuration file to avoid 7 | # parsing problems on the controller 8 | motoros2_config.yaml text eol=crlf 9 | -------------------------------------------------------------------------------- /config/.yamllint: -------------------------------------------------------------------------------- 1 | --- 2 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 3 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 4 | # 5 | # SPDX-License-Identifier: CC0-1.0 6 | extends: default 7 | 8 | yaml-files: 9 | - 'motoros2_config.yaml' 10 | 11 | rules: 12 | line-length: 13 | max: 120 14 | quoted-strings: 15 | quote-type: double 16 | required: only-when-needed 17 | new-lines: 18 | # while not strictly needed, we standardise on Windows-style newlines 19 | # for the configuration file, even when editing the file on Linux or macOS 20 | type: dos 21 | comments: 22 | # disable this as we use comments without starting spaces to visually 23 | # separate the lines with commented keys from explanatory comments 24 | require-starting-space: false 25 | -------------------------------------------------------------------------------- /doc/build_from_source.md: -------------------------------------------------------------------------------- 1 | 7 | 8 | # Building from source code 9 | 10 | Users may want to make changes to the behavior and feature-set of MotoROS2. 11 | To do this, it is necessary to build the MotoROS2 project from source code. 12 | 13 | Please note that building from source code is *not* required for normal usage of MotoROS2. 14 | It is only required when you want to change the included functionality. 15 | 16 | ## Requirements 17 | 18 | - **Microsoft Windows** 19 | - **Microsoft Visual Studio 2017 or higher** (the *Community Edition* is also known to work). 20 | The MotoPlus SDK is not compatible with Visual Studio Code. 21 | - **MotoPlus SDK for Visual Studio v1.5.1 or higher**. 22 | This is not the same as MotoPlus IDE. Users in Asian regions will need to purchase this software from [Yaskawa America directly](https://www.motoman.com/en-us/products/robots/sales-quote). 23 | If you already have a license for an older version of the MotoPlus SDK, please contact `techsupport@motoman.com` or `ccs@yaskawa.eu` for assistance obtaining a newer edition. 24 | - **libmicroros library and headers** that corresponds to your target edition of ROS 2 (Humble, Jazzy, etc) and the Yaskawa controller series you are building MotoROS2 for 25 | Although `libmicroros` as used by MotoROS2 is derived from the open-source [micro-ROS](https://micro.ros.org) project, various patches were needed to make it compatible with MotoPlus. As of writing, these patches can not be made open-source yet. Because of this, we distribute the `libmicroros` dependency as a closed-source library. 26 | These can be found on the [micro_ros_motoplus/Releases](https://github.com/yaskawa-global/micro_ros_motoplus/releases) page. 27 | 28 | ## Setup 29 | 30 | Clone the [yaskawa-global/motoros2](https://github.com/yaskawa-global/motoros2.git) repository. 31 | 32 | Obtain the `libmicroros` and headers for your target ROS 2 variant and controller series. 33 | 34 | These are provided as separate downloads and can be found on the [micro_ros_motoplus/Releases](https://github.com/yaskawa-global/micro_ros_motoplus/releases) page. 35 | 36 | When downloading `libmicroros` from the repo, the folder will be named with a unique identifier corresponding to the time stamp of the build. 37 | Rename the folder to `libmicroros__`. 38 | E.g. `libmicroros_yrc1000_jazzy`. 39 | 40 | Move the folder containing `libmicroros` and the headers inside the folder with the Visual Studio solution (ie: `MotoROS2.sln`). 41 | 42 | Example showing where `libmicroros_yrc1000_jazzy` should be moved for a Jazzy build of MotoROS2 targetting a YRC1000 controller: 43 | 44 | ```text 45 | motoros2 46 | |-- libmicroros_yrc1000_jazzy 47 | | |-- include 48 | | | `-- ... 49 | | |-- build_info.yaml 50 | | `-- libmicroros.yrcLib_jazzy 51 | |-- src 52 | | |-- ... 53 | | `-- MotoROS2_AllControllers.vcxproj 54 | |-- ... 55 | `-- MotoROS2.sln 56 | ``` 57 | 58 | Now open `MotoROS2.sln` in Visual Studio. 59 | 60 | ## Verify project configuration 61 | 62 | The *Configuration Manager* dropdown should be pre-populated with the supported combinations of controller models and ROS 2 variants. 63 | If it is not, you will need to add them manually. 64 | This procedure will also allow you to verify the Visual Studio project configuration is correct. 65 | 66 | 1. In the project tree, expand the `MotoPlus Libraries` > `micro-ROS` folder. 67 | Check for the `libmicroros` library that was obtained in the `Setup` procedure above. 68 | 69 | If is is not in the project tree, right-click the `MotoPlus Libraries` folder in the project tree and select `Add` > `Existing Item`. 70 | 71 | ![add_library.png](img/add_library.png) 72 | 73 | Navigate to the `libmicroros` library file and add it to the project. 74 | 75 | 1. In the top menu, choose `Project` > `Properties`. 76 | Use the `Configuration` dropdown to choose the correct configuration for your robot controller model and ROS 2 variant. 77 | 78 | ![properties_configuration.png](img/properties_configuration.png) 79 | 80 | Choose `VC++ Directories` from the left tree view. 81 | Verify the path to `libmicroros` headers exists in the `Include Directories` field. 82 | This can be a relative path, using MSBuild macros. 83 | Copy this path, as it will be used again in the next step. 84 | 85 | Example: `$(ProjectDir)..\libmicroros_yrc1000_jazzy\include;` 86 | NOTE: Do not use the `$(SolutionDir)` macro in this step. 87 | The path should be either relative to the project file or absolute. 88 | 89 | ![vc_directories.png](img/vc_directories.png) 90 | 91 | 1. Choose `NMake` from the left tree view. 92 | Ensure the start of the `Include Search Path` includes the path to the `libmicroros` headers. 93 | Example: `$(ProjectDir)..\libmicroros_yrc1000_jazzy\include;` 94 | 95 | ![intellisense_search_path.png](img/intellisense_search_path.png) 96 | 97 | Click `OK` to close the properties dialog. 98 | 99 | Repeat these setup instructions for each controller model and ROS 2 variant that you wwant to build MotoROS2 for. 100 | 101 | ## Building 102 | 103 | Use the *Configuration Manager* dropdown to select your target controller model and ROS 2 variant. 104 | 105 | ![configuration_manager.png](img/configuration_manager.png) 106 | 107 | From the main menu, navigate to `Build` > `Build MotoROS2` to build the project. 108 | 109 | Alternatively, if you are targeting multiple configurations, navigate to `Build` > `Batch Build`. 110 | Check each configuration that you are targeting and then click `Build`. 111 | -------------------------------------------------------------------------------- /doc/img/add_library.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/add_library.png -------------------------------------------------------------------------------- /doc/img/configuration_manager.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/configuration_manager.png -------------------------------------------------------------------------------- /doc/img/intellisense_search_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/intellisense_search_path.png -------------------------------------------------------------------------------- /doc/img/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/logo.png -------------------------------------------------------------------------------- /doc/img/properties_configuration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/properties_configuration.png -------------------------------------------------------------------------------- /doc/img/vc_directories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/doc/img/vc_directories.png -------------------------------------------------------------------------------- /lib/CmosParameterTypes.h: -------------------------------------------------------------------------------- 1 | /* ParameterTypes.h - Parameter Extraction type definitions header file */ 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: BSD-3-Clause 7 | 8 | #ifndef MOTOROS2_CMOS_PARAMETER_TYPES_H 9 | #define MOTOROS2_CMOS_PARAMETER_TYPES_H 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | typedef struct 16 | { 17 | double PtoR[MAX_PULSE_AXES]; //Array to store PULSE TO RADIAN conversion factors for each axes 18 | } PULSE_TO_RAD; 19 | 20 | typedef struct 21 | { 22 | double PtoM[MAX_PULSE_AXES]; //Array to store PULSE TO METER conversion factors for each axes 23 | } PULSE_TO_METER; 24 | 25 | typedef enum 26 | { 27 | AXIS_ROTATION, 28 | AXIS_LINEAR, 29 | AXIS_INVALID 30 | } AXIS_TYPE; 31 | 32 | typedef struct 33 | { 34 | AXIS_TYPE type[MAX_PULSE_AXES]; //Array to store whether axis is rotational or linear 35 | } AXIS_MOTION_TYPE; 36 | 37 | typedef struct 38 | { 39 | BOOL bValid; //TRUE if ulSourceAxis != 0 40 | INT32 ulSourceAxis; 41 | INT32 ulCorrectionAxis; 42 | double fCorrectionRatio; 43 | } FB_AXIS_CORRECTION; 44 | 45 | typedef struct 46 | { 47 | FB_AXIS_CORRECTION correction[MAX_PULSE_AXES]; 48 | } FB_PULSE_CORRECTION_DATA; 49 | 50 | typedef struct 51 | { 52 | UINT32 qtyOfOutFiles; 53 | UINT32 qtyOfHighPriorityTasks; 54 | UINT32 qtyOfNormalPriorityTasks; 55 | } TASK_QTY_INFO; 56 | 57 | typedef struct 58 | { 59 | UINT16 periodInMilliseconds; 60 | } GP_INTERPOLATION_PERIOD; 61 | 62 | typedef struct 63 | { 64 | UINT32 maxIncrement[MAX_PULSE_AXES]; 65 | } MAX_INCREMENT_INFO; 66 | 67 | typedef struct 68 | { 69 | int ctrlGrp; //Robot control group 70 | int IpCycleInMilliseconds; //Interpolation Cycle in milliseconds 71 | MAX_INCREMENT_INFO info; //Maximum increment per interpolation cycle 72 | } MAX_INC_PIPC; 73 | 74 | typedef struct 75 | { 76 | INT32 maxLimit[MAX_PULSE_AXES]; 77 | INT32 minLimit[MAX_PULSE_AXES]; 78 | } JOINT_PULSE_LIMITS; 79 | 80 | typedef struct 81 | { 82 | INT32 maxLimit[MAX_PULSE_AXES]; 83 | } JOINT_ANGULAR_VELOCITY_LIMITS; 84 | 85 | typedef struct 86 | { 87 | BOOL bFeedbackSpeedEnabled; 88 | ULONG cioAddressForAxis[MAX_PULSE_AXES][2]; 89 | } JOINT_FEEDBACK_SPEED_ADDRESSES; 90 | 91 | typedef struct 92 | { 93 | float theta; 94 | float d; 95 | float a; 96 | float alpha; 97 | } DH_LINK; 98 | 99 | typedef struct 100 | { 101 | DH_LINK link[MAX_PULSE_AXES]; 102 | } DH_PARAMETERS; 103 | 104 | typedef enum 105 | { 106 | MOTION_TYPE_NOT_USED = -1, 107 | MOTION_TYPE_X, 108 | MOTION_TYPE_Y, 109 | MOTION_TYPE_Z, 110 | MOTION_TYPE_RX, 111 | MOTION_TYPE_RY, 112 | MOTION_TYPE_RZ 113 | } BASE_AXIS_MOTION_TYPE; 114 | 115 | typedef struct 116 | { 117 | BASE_AXIS_MOTION_TYPE motionType[MAX_PULSE_AXES]; 118 | MP_COORD offsetFromBaseToRobotOrigin; 119 | } BASE_AXIS_INFO; 120 | 121 | typedef enum 122 | { 123 | ECO_UNIT_MINUTES, 124 | ECO_UNIT_SECONDS 125 | } ECO_MODE_UNITS; 126 | 127 | typedef struct 128 | { 129 | BOOL bEnabled; 130 | ECO_MODE_UNITS timeUnit; 131 | UINT32 timeout; 132 | } ECO_MODE_INFO; 133 | 134 | #ifdef __cplusplus 135 | } 136 | #endif 137 | 138 | #endif // MOTOROS2_CMOS_PARAMETER_TYPES_H 139 | -------------------------------------------------------------------------------- /lib/MotoPlusExterns.h: -------------------------------------------------------------------------------- 1 | //MotoPlusExterns.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MOTOPLUS_EXTERNS_H 9 | #define MOTOROS2_MOTOPLUS_EXTERNS_H 10 | 11 | 12 | #define MP_MEM_PART_SIZE 1048576 13 | 14 | extern size_t mpNumBytesFree(void); 15 | 16 | #if defined (YRC1000) || defined (YRC1000u) 17 | extern MP_GRP_ID_TYPE mpCtrlGrpNo2GrpId(int grp_no); 18 | #endif 19 | 20 | //M+ only defines MAX_ALARM_COUNT, but we'd like to avoid magic nrs 21 | //as much as possible, so we define our own here for errors. 22 | //But only if something hasn't been defined already. 23 | #ifndef MAX_ERROR_COUNT 24 | #define MAX_ERROR_COUNT 1 25 | #endif 26 | 27 | #ifndef E_EXRCS_PFL_FUNC_BUSY 28 | #define E_EXRCS_PFL_FUNC_BUSY (-19) 29 | #endif 30 | 31 | #ifndef E_EXRCS_UNDER_ENERGY_SAVING 32 | #define E_EXRCS_UNDER_ENERGY_SAVING (-20) 33 | #endif 34 | 35 | //from the M+ API Function Specifications on mpSetCycle(..) (HW1483602) 36 | #define MP_CYCLE_MODE_STEP 1 37 | #define MP_CYCLE_MODE_1CYCLE 2 38 | #define MP_CYCLE_MODE_AUTO 3 39 | 40 | 41 | #endif // MOTOROS2_MOTOPLUS_EXTERNS_H 42 | -------------------------------------------------------------------------------- /lib/MotoROS_PlatformLib.dnLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/MotoROS_PlatformLib.dnLib -------------------------------------------------------------------------------- /lib/MotoROS_PlatformLib.h: -------------------------------------------------------------------------------- 1 | // MotoROS_PlatformLib.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS_PLATFORM_LIB_H 9 | #define MOTOROS_PLATFORM_LIB_H 10 | 11 | 12 | // Define the MotoROS_PlatformLib version number as a base-10 integer. 13 | // 14 | // Format: XXXYYYZZ, where: 15 | // 16 | // - XXX: major version 17 | // - YYY: minor version, and 18 | // - ZZ: patch version nr 19 | // 20 | // Note: no leading zeros, to avoid octal parsing 21 | #define MOTOROS_PLATFORM_LIB_VERSION 212 22 | 23 | #define MOTOROS_PLATFORM_LIB_MAJOR (MOTOROS_PLATFORM_LIB_VERSION / 100000) 24 | #define MOTOROS_PLATFORM_LIB_MINOR (MOTOROS_PLATFORM_LIB_VERSION / 100 % 1000) 25 | #define MOTOROS_PLATFORM_LIB_PATCH (MOTOROS_PLATFORM_LIB_VERSION % 100) 26 | 27 | 28 | // Returns the PlatformLib version number 29 | extern UINT32 MotoROS_PlatformLib_GetVersion(); 30 | 31 | 32 | extern BOOL Ros_IsOtherInstanceRunning(); 33 | 34 | 35 | // Attempt to read MAC address from the specified LAN interface 36 | // IFF return value is 'OK', 'macAddress' will contain the MAC. In all other 37 | // cases, 'macAddress' may be left uninitialised. 38 | #define ROS_GET_MAC_ADDR_INVALID_IF_NO -10 39 | #define ROS_GET_MAC_ADDR_GENERIC_FAILURE -11 40 | extern STATUS Ros_GetMacAddress(USHORT if_no, UINT8 macAddress[6]); 41 | 42 | 43 | #define ROS_USER_LAN1 1 /* general LAN interface1 */ 44 | #define ROS_USER_LAN2 2 /* general LAN interface2(only YRC1000) */ 45 | 46 | // Retrieves the IP address, subnet mask, MAC address and default gateway for the specified network interface. 47 | // NOTE: If this is called immediately upon startup, the default_gw may not be available yet. In this case, the default_gw will be set to 0. 48 | #define ROS_MP_NIC_DATA_INVALID_IF_NO -10 49 | #define ROS_MP_NIC_DATA_ERR_IP_ADDR -11 50 | #define ROS_MP_NIC_DATA_ERR_IP_ADDR_SZ -12 51 | #define ROS_MP_NIC_DATA_ERR_INADDR -13 52 | #define ROS_MP_NIC_DATA_ERR_MASK -14 53 | // allow room for reporting Ros_GetMacAddress(..) error codes 54 | #define ROS_MP_NIC_DATA_ERR_MAC -100 55 | // allow room for reporting Ros_GetGateway(..) error codes 56 | #define ROS_MP_NIC_DATA_ERR_GW -200 57 | extern STATUS Ros_mpNICData(USHORT if_no, ULONG* ip_addr, ULONG* subnet_mask, UCHAR* mac_addr, ULONG* default_gw); 58 | 59 | 60 | extern size_t Ros_strnlen(const char *s, size_t maxlen); 61 | 62 | 63 | extern ULONG tickGet(); 64 | 65 | 66 | #define SO_BROADCAST 0x0020 67 | extern STATUS Ros_setsockopt(int s, int level, int optname, char* optval, int optlen); 68 | 69 | #if defined (DX100) || defined (FS100) 70 | // VxWorks 5.5/6.8 71 | extern int localtime_r(const time_t* timer, struct tm* timeBuffer); 72 | #elif defined (YRC1000) || defined (YRC1000u) || defined (DX200) 73 | // >= VxWorks 6.9 74 | struct tm* localtime_r(const time_t* timep, struct tm* result); 75 | #else 76 | #error localtime_r: unsupported platform 77 | #endif 78 | 79 | 80 | #if defined (YRC1000) || defined (YRC1000u) || defined (DX200) || defined (FS100) || defined (DX100) 81 | // from clockLib 82 | struct timespec 83 | { 84 | time_t tv_sec; /* seconds */ 85 | long tv_nsec; /* nanoseconds (0 -1,000,000,000) */ 86 | }; 87 | #ifndef CLOCK_REALTIME 88 | #define CLOCK_REALTIME 0 89 | #endif 90 | typedef int clockid_t; 91 | extern int clock_gettime(clockid_t clock_id, /* clock ID (always CLOCK_REALTIME) */ 92 | struct timespec* tp /* where to store current time */); 93 | #else 94 | #error clock_gettime: unsupported platform 95 | #endif 96 | 97 | 98 | // Attempts to determine whether the specific link is UP (ie: cable is connected). 99 | // IFF return value is 'OK', 'is_up' will reflect link state. In all other cases, 100 | // 'is_up' may be left uninitialised. 101 | #define ROS_USERLAN_STATE_INVALID_IF_NO -10 102 | #define ROS_USERLAN_STATE_NO_SUCH_IF -11 103 | #define ROS_USERLAN_STATE_GET_STATE_ERROR -12 104 | #define ROS_USERLAN_STATE_GENERIC_FAILURE -13 105 | extern STATUS Ros_UserLan_IsLinkUp(USHORT if_no, BOOL* const is_up /*out*/); 106 | 107 | 108 | #endif // MOTOROS_PLATFORM_LIB_H 109 | -------------------------------------------------------------------------------- /lib/MotoROS_PlatformLib.yrcLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/MotoROS_PlatformLib.yrcLib -------------------------------------------------------------------------------- /lib/MotoROS_PlatformLib.yrcmLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/MotoROS_PlatformLib.yrcmLib -------------------------------------------------------------------------------- /lib/ParameterExtraction.dnLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/ParameterExtraction.dnLib -------------------------------------------------------------------------------- /lib/ParameterExtraction.yrcLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/ParameterExtraction.yrcLib -------------------------------------------------------------------------------- /lib/ParameterExtraction.yrcmLib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/lib/ParameterExtraction.yrcmLib -------------------------------------------------------------------------------- /robot_jobs/.gitattributes: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | # Job files must have CRLF (ie: Windows) line-endings. 7 | # avoids "ERROR 3200 : NOP or END instruction not found" errors when 8 | # loading job files onto the controller. 9 | *.JBI text eol=crlf 10 | 11 | *.DAT text eol=crlf 12 | -------------------------------------------------------------------------------- /robot_jobs/README.md: -------------------------------------------------------------------------------- 1 | 7 | 8 | # Example INFORM jobs 9 | 10 | As stated [in the main README](../README.md#example-inform-jobs), the files in these directories are not (directly) needed by MotoROS2, and are only provided as examples in case custom INFORM jobs are needed. 11 | -------------------------------------------------------------------------------- /robot_jobs/sda_dual_arm/INIT_ROS.JBI: -------------------------------------------------------------------------------- 1 | /JOB 2 | //NAME INIT_ROS 3 | //POS 4 | ///NPOS 0,0,0,0,0,0 5 | //INST 6 | ///DATE 2014/06/16 21:00 7 | ///ATTR SC,RW 8 | ///GROUP1 RB2,BS2 9 | ///GROUP2 RB1,BS1 10 | NOP 11 | DOUT OT#(890) OFF 12 | DOUT OT#(889) OFF 13 | TIMER T=0.05 14 | DOUT OT#(889) ON 15 | WAIT OT#(890)=ON 16 | DOUT OT#(890) OFF 17 | DOUT OT#(889) OFF 18 | END 19 | -------------------------------------------------------------------------------- /robot_jobs/sda_dual_arm/IONAME.DAT: -------------------------------------------------------------------------------- 1 | /IONAME 2 | //IN 3 | ,,, 4 | ,,, 5 | ,,, 6 | ,,, 7 | ,,, 8 | ,,, 9 | ,,, 10 | ,,, 11 | ,,, 12 | ,,, 13 | ,,, 14 | ,,, 15 | ,,, 16 | ,,, 17 | ,,, 18 | ,,, 19 | ,,, 20 | ,,, 21 | ,,, 22 | ,,, 23 | ,,, 24 | ,,, 25 | ,,, 26 | ,,, 27 | ,,, 28 | ,,, 29 | ,,, 30 | ,,, 31 | ,,, 32 | ,,, 33 | ,,, 34 | ,,, 35 | ,,, 36 | ,,, 37 | ,,, 38 | ,,, 39 | ,,, 40 | ,,, 41 | ,,, 42 | ,,, 43 | ,,, 44 | ,,, 45 | ,,, 46 | ,,, 47 | ,,, 48 | ,,, 49 | ,,, 50 | ,,, 51 | ,,, 52 | ,,, 53 | ,,, 54 | ,,, 55 | ,,, 56 | ,,, 57 | ,,, 58 | ,,, 59 | ,,, 60 | ,,, 61 | ,,, 62 | ,,, 63 | ,,, 64 | ,,, 65 | ,,, 66 | ,,, 67 | ,,, 68 | ,,, 69 | ,,, 70 | ,,, 71 | ,,, 72 | ,,, 73 | ,,, 74 | ,,, 75 | ,,, 76 | ,,, 77 | ,,, 78 | ,,, 79 | ,,, 80 | ,,, 81 | ,,, 82 | ,,, 83 | ,,, 84 | ,,, 85 | ,,, 86 | ,,, 87 | ,,, 88 | ,,, 89 | ,,, 90 | ,,, 91 | ,,, 92 | ,,, 93 | ,,, 94 | ,,, 95 | ,,, 96 | ,,, 97 | ,,, 98 | ,,, 99 | ,,, 100 | ,,, 101 | ,,, 102 | ,,, 103 | ,,, 104 | ,,, 105 | ,,, 106 | ,,, 107 | ,,, 108 | ,,, 109 | ,,, 110 | ,,, 111 | ,,, 112 | ,,, 113 | ,,, 114 | ,,, 115 | ,,, 116 | ,,, 117 | ,,, 118 | ,,, 119 | ,,, 120 | ,,, 121 | ,,, 122 | ,,, 123 | ,,, 124 | ,,, 125 | ,,, 126 | ,,, 127 | ,,, 128 | ,,, 129 | ,,, 130 | ,,, 131 | ,,, 132 | ,,, 133 | ,,, 134 | ,,, 135 | ,,, 136 | ,,, 137 | ,,, 138 | ,,, 139 | ,,, 140 | ,,, 141 | ,,, 142 | ,,, 143 | ,,, 144 | ,,, 145 | ,,, 146 | ,,, 147 | ,,, 148 | ,,, 149 | ,,, 150 | ,,, 151 | ,,, 152 | ,,, 153 | ,,, 154 | ,,, 155 | ,,, 156 | ,,, 157 | ,,, 158 | ,,, 159 | ,,, 160 | ,,, 161 | ,,, 162 | ,,, 163 | ,,, 164 | ,,, 165 | ,,, 166 | ,,, 167 | ,,, 168 | ,,, 169 | ,,, 170 | ,,, 171 | ,,, 172 | ,,, 173 | ,,, 174 | ,,, 175 | ,,, 176 | ,,, 177 | ,,, 178 | ,,, 179 | ,,, 180 | ,,, 181 | ,,, 182 | ,,, 183 | ,,, 184 | ,,, 185 | ,,, 186 | ,,, 187 | ,,, 188 | ,,, 189 | ,,, 190 | ,,, 191 | ,,, 192 | ,,, 193 | ,,, 194 | ,,, 195 | ,,, 196 | ,,, 197 | ,,, 198 | ,,, 199 | ,,, 200 | ,,, 201 | ,,, 202 | ,,, 203 | ,,, 204 | ,,, 205 | ,,, 206 | ,,, 207 | ,,, 208 | ,,, 209 | ,,, 210 | ,,, 211 | ,,, 212 | ,,, 213 | ,,, 214 | ,,, 215 | ,,, 216 | ,,, 217 | ,,, 218 | ,,, 219 | ,,, 220 | ,,, 221 | ,,, 222 | ,,, 223 | ,,, 224 | ,,, 225 | ,,, 226 | ,,, 227 | ,,, 228 | ,,, 229 | ,,, 230 | ,,, 231 | ,,, 232 | ,,, 233 | ,,, 234 | ,,, 235 | ,,, 236 | ,,, 237 | ,,, 238 | ,,, 239 | ,,, 240 | ,,, 241 | ,,, 242 | ,,, 243 | ,,, 244 | ,,, 245 | ,,, 246 | ,,, 247 | ,,, 248 | ,,, 249 | ,,, 250 | ,,, 251 | ,,, 252 | ,,, 253 | ,,, 254 | ,,, 255 | ,,, 256 | ,,, 257 | ,,, 258 | ,,, 259 | //OUT 260 | ,,, 261 | ,,, 262 | ,,, 263 | ,,, 264 | ,,, 265 | ,,, 266 | ,,, 267 | ,,, 268 | ,,, 269 | ,,, 270 | ,,, 271 | ,,, 272 | ,,, 273 | ,,, 274 | ,,, 275 | ,,, 276 | ,,, 277 | ,,, 278 | ,,, 279 | ,,, 280 | ,,, 281 | ,,, 282 | ,,, 283 | ,,, 284 | ,,, 285 | ,,, 286 | ,,, 287 | ,,, 288 | ,,, 289 | ,,, 290 | ,,, 291 | ,,, 292 | ,,, 293 | ,,, 294 | ,,, 295 | ,,, 296 | ,,, 297 | ,,, 298 | ,,, 299 | ,,, 300 | ,,, 301 | ,,, 302 | ,,, 303 | ,,, 304 | ,,, 305 | ,,, 306 | ,,, 307 | ,,, 308 | ,,, 309 | ,,, 310 | ,,, 311 | ,,, 312 | ,,, 313 | ,,, 314 | ,,, 315 | ,,, 316 | ,,, 317 | ,,, 318 | ,,, 319 | ,,, 320 | ,,, 321 | ,,, 322 | ,,, 323 | ,,, 324 | ,,, 325 | ,,, 326 | ,,, 327 | ,,, 328 | ,,, 329 | ,,, 330 | ,,, 331 | ,,, 332 | ,,, 333 | ,,, 334 | ,,, 335 | ,,, 336 | ,,, 337 | ,,, 338 | ,,, 339 | ,,, 340 | ,,, 341 | ,,, 342 | ,,, 343 | ,,, 344 | ,,, 345 | ,,, 346 | ,,, 347 | ,,, 348 | ,,, 349 | ,,, 350 | ,,, 351 | ,,, 352 | ,,, 353 | ,,, 354 | ,,, 355 | ,,, 356 | ,,, 357 | ,,, 358 | ,,, 359 | ,,, 360 | ,,, 361 | ,,, 362 | ,,, 363 | ,,, 364 | ,,, 365 | ,,, 366 | ,,, 367 | ,,, 368 | ,,, 369 | ,,, 370 | ,,, 371 | ,,, 372 | ,,, 373 | ,,, 374 | ,,, 375 | ,,, 376 | ,,, 377 | ,,, 378 | ,,, 379 | ,,, 380 | ,,, 381 | ,,, 382 | ,,, 383 | ,,, 384 | ,,, 385 | ,,, 386 | ,,, 387 | ,,, 388 | ,,, 389 | ,,, 390 | ,,, 391 | ,,, 392 | ,,, 393 | ,,, 394 | ,,, 395 | ,,, 396 | ,,, 397 | ,,, 398 | ,,, 399 | ,,, 400 | ,,, 401 | ,,, 402 | ,,, 403 | ,,, 404 | ,,, 405 | ,,, 406 | ,,, 407 | ,,, 408 | ,,, 409 | ,,, 410 | ,,, 411 | ,,, 412 | ,,, 413 | ,,, 414 | ,,, 415 | ,,, 416 | ,,, 417 | ,,, 418 | ,,, 419 | ,,, 420 | ,,, 421 | ,,, 422 | ,,, 423 | ,,, 424 | ,,, 425 | ,,, 426 | ,,, 427 | ,,, 428 | ,,, 429 | ,,, 430 | ,,, 431 | ,,, 432 | ,,, 433 | ,,, 434 | ,,, 435 | ,,, 436 | ,,, 437 | ,,, 438 | ,,, 439 | ,,, 440 | ,,, 441 | ,,, 442 | ,,, 443 | ,,, 444 | ,,, 445 | ,,, 446 | ,,, 447 | ,,, 448 | ,,, 449 | ,,, 450 | ,,, 451 | ,,, 452 | ,,, 453 | ,,, 454 | ,,, 455 | ,,, 456 | ,,, 457 | ,,, 458 | ,,, 459 | ,,, 460 | ,,, 461 | ,,, 462 | ,,, 463 | ,,, 464 | ,,, 465 | ,,, 466 | ,,, 467 | ,,, 468 | ,,, 469 | ,,, 470 | ,,, 471 | ,,, 472 | ,,, 473 | ,,, 474 | ,,, 475 | ,,, 476 | ,,, 477 | ,,, 478 | ,,, 479 | ,,, 480 | ,,, 481 | ,,, 482 | ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV 483 | MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE 484 | ,,, 485 | ,,, 486 | ,,, 487 | ,,, 488 | ,,, 489 | ,,, 490 | ,,, 491 | ,,, 492 | ,,, 493 | ,,, 494 | ,,, 495 | ,,, 496 | ,,, 497 | ,,, 498 | ,,, 499 | ,,, 500 | ,,, 501 | ,,, 502 | ,,, 503 | ,,, 504 | ,,, 505 | ,,, 506 | ,,, 507 | ,,, 508 | ,,, 509 | ,,, 510 | ,,, 511 | ,,, 512 | ,,, 513 | ,,, 514 | ,,, 515 | ,,, 516 | -------------------------------------------------------------------------------- /robot_jobs/single_arm/INIT_ROS.JBI: -------------------------------------------------------------------------------- 1 | /JOB 2 | //NAME INIT_ROS 3 | //POS 4 | ///NPOS 0,0,0,0,0,0 5 | //INST 6 | ///DATE 2013/03/29 05:32 7 | ///ATTR SC,RW 8 | ///GROUP1 RB1 9 | NOP 10 | DOUT OT#(890) OFF 11 | DOUT OT#(889) OFF 12 | TIMER T=0.05 13 | DOUT OT#(889) ON 14 | WAIT OT#(890)=ON 15 | DOUT OT#(890) OFF 16 | DOUT OT#(889) OFF 17 | END 18 | -------------------------------------------------------------------------------- /robot_jobs/single_arm/IONAME.DAT: -------------------------------------------------------------------------------- 1 | /IONAME 2 | //IN 3 | ,,, 4 | ,,, 5 | ,,, 6 | ,,, 7 | ,,, 8 | ,,, 9 | ,,, 10 | ,,, 11 | ,,, 12 | ,,, 13 | ,,, 14 | ,,, 15 | ,,, 16 | ,,, 17 | ,,, 18 | ,,, 19 | ,,, 20 | ,,, 21 | ,,, 22 | ,,, 23 | ,,, 24 | ,,, 25 | ,,, 26 | ,,, 27 | ,,, 28 | ,,, 29 | ,,, 30 | ,,, 31 | ,,, 32 | ,,, 33 | ,,, 34 | ,,, 35 | ,,, 36 | ,,, 37 | ,,, 38 | ,,, 39 | ,,, 40 | ,,, 41 | ,,, 42 | ,,, 43 | ,,, 44 | ,,, 45 | ,,, 46 | ,,, 47 | ,,, 48 | ,,, 49 | ,,, 50 | ,,, 51 | ,,, 52 | ,,, 53 | ,,, 54 | ,,, 55 | ,,, 56 | ,,, 57 | ,,, 58 | ,,, 59 | ,,, 60 | ,,, 61 | ,,, 62 | ,,, 63 | ,,, 64 | ,,, 65 | ,,, 66 | ,,, 67 | ,,, 68 | ,,, 69 | ,,, 70 | ,,, 71 | ,,, 72 | ,,, 73 | ,,, 74 | ,,, 75 | ,,, 76 | ,,, 77 | ,,, 78 | ,,, 79 | ,,, 80 | ,,, 81 | ,,, 82 | ,,, 83 | ,,, 84 | ,,, 85 | ,,, 86 | ,,, 87 | ,,, 88 | ,,, 89 | ,,, 90 | ,,, 91 | ,,, 92 | ,,, 93 | ,,, 94 | ,,, 95 | ,,, 96 | ,,, 97 | ,,, 98 | ,,, 99 | ,,, 100 | ,,, 101 | ,,, 102 | ,,, 103 | ,,, 104 | ,,, 105 | ,,, 106 | ,,, 107 | ,,, 108 | ,,, 109 | ,,, 110 | ,,, 111 | ,,, 112 | ,,, 113 | ,,, 114 | ,,, 115 | ,,, 116 | ,,, 117 | ,,, 118 | ,,, 119 | ,,, 120 | ,,, 121 | ,,, 122 | ,,, 123 | ,,, 124 | ,,, 125 | ,,, 126 | ,,, 127 | ,,, 128 | ,,, 129 | ,,, 130 | ,,, 131 | ,,, 132 | ,,, 133 | ,,, 134 | ,,, 135 | ,,, 136 | ,,, 137 | ,,, 138 | ,,, 139 | ,,, 140 | ,,, 141 | ,,, 142 | ,,, 143 | ,,, 144 | ,,, 145 | ,,, 146 | ,,, 147 | ,,, 148 | ,,, 149 | ,,, 150 | ,,, 151 | ,,, 152 | ,,, 153 | ,,, 154 | ,,, 155 | ,,, 156 | ,,, 157 | ,,, 158 | ,,, 159 | ,,, 160 | ,,, 161 | ,,, 162 | ,,, 163 | ,,, 164 | ,,, 165 | ,,, 166 | ,,, 167 | ,,, 168 | ,,, 169 | ,,, 170 | ,,, 171 | ,,, 172 | ,,, 173 | ,,, 174 | ,,, 175 | ,,, 176 | ,,, 177 | ,,, 178 | ,,, 179 | ,,, 180 | ,,, 181 | ,,, 182 | ,,, 183 | ,,, 184 | ,,, 185 | ,,, 186 | ,,, 187 | ,,, 188 | ,,, 189 | ,,, 190 | ,,, 191 | ,,, 192 | ,,, 193 | ,,, 194 | ,,, 195 | ,,, 196 | ,,, 197 | ,,, 198 | ,,, 199 | ,,, 200 | ,,, 201 | ,,, 202 | ,,, 203 | ,,, 204 | ,,, 205 | ,,, 206 | ,,, 207 | ,,, 208 | ,,, 209 | ,,, 210 | ,,, 211 | ,,, 212 | ,,, 213 | ,,, 214 | ,,, 215 | ,,, 216 | ,,, 217 | ,,, 218 | ,,, 219 | ,,, 220 | ,,, 221 | ,,, 222 | ,,, 223 | ,,, 224 | ,,, 225 | ,,, 226 | ,,, 227 | ,,, 228 | ,,, 229 | ,,, 230 | ,,, 231 | ,,, 232 | ,,, 233 | ,,, 234 | ,,, 235 | ,,, 236 | ,,, 237 | ,,, 238 | ,,, 239 | ,,, 240 | ,,, 241 | ,,, 242 | ,,, 243 | ,,, 244 | ,,, 245 | ,,, 246 | ,,, 247 | ,,, 248 | ,,, 249 | ,,, 250 | ,,, 251 | ,,, 252 | ,,, 253 | ,,, 254 | ,,, 255 | ,,, 256 | ,,, 257 | ,,, 258 | ,,, 259 | //OUT 260 | ,,, 261 | ,,, 262 | ,,, 263 | ,,, 264 | ,,, 265 | ,,, 266 | ,,, 267 | ,,, 268 | ,,, 269 | ,,, 270 | ,,, 271 | ,,, 272 | ,,, 273 | ,,, 274 | ,,, 275 | ,,, 276 | ,,, 277 | ,,, 278 | ,,, 279 | ,,, 280 | ,,, 281 | ,,, 282 | ,,, 283 | ,,, 284 | ,,, 285 | ,,, 286 | ,,, 287 | ,,, 288 | ,,, 289 | ,,, 290 | ,,, 291 | ,,, 292 | ,,, 293 | ,,, 294 | ,,, 295 | ,,, 296 | ,,, 297 | ,,, 298 | ,,, 299 | ,,, 300 | ,,, 301 | ,,, 302 | ,,, 303 | ,,, 304 | ,,, 305 | ,,, 306 | ,,, 307 | ,,, 308 | ,,, 309 | ,,, 310 | ,,, 311 | ,,, 312 | ,,, 313 | ,,, 314 | ,,, 315 | ,,, 316 | ,,, 317 | ,,, 318 | ,,, 319 | ,,, 320 | ,,, 321 | ,,, 322 | ,,, 323 | ,,, 324 | ,,, 325 | ,,, 326 | ,,, 327 | ,,, 328 | ,,, 329 | ,,, 330 | ,,, 331 | ,,, 332 | ,,, 333 | ,,, 334 | ,,, 335 | ,,, 336 | ,,, 337 | ,,, 338 | ,,, 339 | ,,, 340 | ,,, 341 | ,,, 342 | ,,, 343 | ,,, 344 | ,,, 345 | ,,, 346 | ,,, 347 | ,,, 348 | ,,, 349 | ,,, 350 | ,,, 351 | ,,, 352 | ,,, 353 | ,,, 354 | ,,, 355 | ,,, 356 | ,,, 357 | ,,, 358 | ,,, 359 | ,,, 360 | ,,, 361 | ,,, 362 | ,,, 363 | ,,, 364 | ,,, 365 | ,,, 366 | ,,, 367 | ,,, 368 | ,,, 369 | ,,, 370 | ,,, 371 | ,,, 372 | ,,, 373 | ,,, 374 | ,,, 375 | ,,, 376 | ,,, 377 | ,,, 378 | ,,, 379 | ,,, 380 | ,,, 381 | ,,, 382 | ,,, 383 | ,,, 384 | ,,, 385 | ,,, 386 | ,,, 387 | ,,, 388 | ,,, 389 | ,,, 390 | ,,, 391 | ,,, 392 | ,,, 393 | ,,, 394 | ,,, 395 | ,,, 396 | ,,, 397 | ,,, 398 | ,,, 399 | ,,, 400 | ,,, 401 | ,,, 402 | ,,, 403 | ,,, 404 | ,,, 405 | ,,, 406 | ,,, 407 | ,,, 408 | ,,, 409 | ,,, 410 | ,,, 411 | ,,, 412 | ,,, 413 | ,,, 414 | ,,, 415 | ,,, 416 | ,,, 417 | ,,, 418 | ,,, 419 | ,,, 420 | ,,, 421 | ,,, 422 | ,,, 423 | ,,, 424 | ,,, 425 | ,,, 426 | ,,, 427 | ,,, 428 | ,,, 429 | ,,, 430 | ,,, 431 | ,,, 432 | ,,, 433 | ,,, 434 | ,,, 435 | ,,, 436 | ,,, 437 | ,,, 438 | ,,, 439 | ,,, 440 | ,,, 441 | ,,, 442 | ,,, 443 | ,,, 444 | ,,, 445 | ,,, 446 | ,,, 447 | ,,, 448 | ,,, 449 | ,,, 450 | ,,, 451 | ,,, 452 | ,,, 453 | ,,, 454 | ,,, 455 | ,,, 456 | ,,, 457 | ,,, 458 | ,,, 459 | ,,, 460 | ,,, 461 | ,,, 462 | ,,, 463 | ,,, 464 | ,,, 465 | ,,, 466 | ,,, 467 | ,,, 468 | ,,, 469 | ,,, 470 | ,,, 471 | ,,, 472 | ,,, 473 | ,,, 474 | ,,, 475 | ,,, 476 | ,,, 477 | ,,, 478 | ,,, 479 | ,,, 480 | ,,, 481 | ,,, 482 | ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV 483 | MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE 484 | ,,, 485 | ,,, 486 | ,,, 487 | ,,, 488 | ,,, 489 | ,,, 490 | ,,, 491 | ,,, 492 | ,,, 493 | ,,, 494 | ,,, 495 | ,,, 496 | ,,, 497 | ,,, 498 | ,,, 499 | ,,, 500 | ,,, 501 | ,,, 502 | ,,, 503 | ,,, 504 | ,,, 505 | ,,, 506 | ,,, 507 | ,,, 508 | ,,, 509 | ,,, 510 | ,,, 511 | ,,, 512 | ,,, 513 | ,,, 514 | ,,, 515 | ,,, 516 | -------------------------------------------------------------------------------- /robot_jobs/single_arm_with_base_axis/INIT_ROS.JBI: -------------------------------------------------------------------------------- 1 | /JOB 2 | //NAME INIT_ROS 3 | //POS 4 | ///NPOS 0,0,0,0,0,0 5 | //INST 6 | ///DATE 2012/01/01 06:53 7 | ///ATTR SC,RW 8 | ///GROUP1 RB1,BS1 9 | NOP 10 | DOUT OT#(890) OFF 11 | DOUT OT#(889) OFF 12 | TIMER T=0.05 13 | DOUT OT#(889) ON 14 | WAIT OT#(890)=ON 15 | DOUT OT#(890) OFF 16 | DOUT OT#(889) OFF 17 | END 18 | -------------------------------------------------------------------------------- /robot_jobs/single_arm_with_base_axis/IONAME.DAT: -------------------------------------------------------------------------------- 1 | /IONAME 2 | //IN 3 | ,,, 4 | ,,, 5 | ,,, 6 | ,,, 7 | ,,, 8 | ,,, 9 | ,,, 10 | ,,, 11 | ,,, 12 | ,,, 13 | ,,, 14 | ,,, 15 | ,,, 16 | ,,, 17 | ,,, 18 | ,,, 19 | ,,, 20 | ,,, 21 | ,,, 22 | ,,, 23 | ,,, 24 | ,,, 25 | ,,, 26 | ,,, 27 | ,,, 28 | ,,, 29 | ,,, 30 | ,,, 31 | ,,, 32 | ,,, 33 | ,,, 34 | ,,, 35 | ,,, 36 | ,,, 37 | ,,, 38 | ,,, 39 | ,,, 40 | ,,, 41 | ,,, 42 | ,,, 43 | ,,, 44 | ,,, 45 | ,,, 46 | ,,, 47 | ,,, 48 | ,,, 49 | ,,, 50 | ,,, 51 | ,,, 52 | ,,, 53 | ,,, 54 | ,,, 55 | ,,, 56 | ,,, 57 | ,,, 58 | ,,, 59 | ,,, 60 | ,,, 61 | ,,, 62 | ,,, 63 | ,,, 64 | ,,, 65 | ,,, 66 | ,,, 67 | ,,, 68 | ,,, 69 | ,,, 70 | ,,, 71 | ,,, 72 | ,,, 73 | ,,, 74 | ,,, 75 | ,,, 76 | ,,, 77 | ,,, 78 | ,,, 79 | ,,, 80 | ,,, 81 | ,,, 82 | ,,, 83 | ,,, 84 | ,,, 85 | ,,, 86 | ,,, 87 | ,,, 88 | ,,, 89 | ,,, 90 | ,,, 91 | ,,, 92 | ,,, 93 | ,,, 94 | ,,, 95 | ,,, 96 | ,,, 97 | ,,, 98 | ,,, 99 | ,,, 100 | ,,, 101 | ,,, 102 | ,,, 103 | ,,, 104 | ,,, 105 | ,,, 106 | ,,, 107 | ,,, 108 | ,,, 109 | ,,, 110 | ,,, 111 | ,,, 112 | ,,, 113 | ,,, 114 | ,,, 115 | ,,, 116 | ,,, 117 | ,,, 118 | ,,, 119 | ,,, 120 | ,,, 121 | ,,, 122 | ,,, 123 | ,,, 124 | ,,, 125 | ,,, 126 | ,,, 127 | ,,, 128 | ,,, 129 | ,,, 130 | ,,, 131 | ,,, 132 | ,,, 133 | ,,, 134 | ,,, 135 | ,,, 136 | ,,, 137 | ,,, 138 | ,,, 139 | ,,, 140 | ,,, 141 | ,,, 142 | ,,, 143 | ,,, 144 | ,,, 145 | ,,, 146 | ,,, 147 | ,,, 148 | ,,, 149 | ,,, 150 | ,,, 151 | ,,, 152 | ,,, 153 | ,,, 154 | ,,, 155 | ,,, 156 | ,,, 157 | ,,, 158 | ,,, 159 | ,,, 160 | ,,, 161 | ,,, 162 | ,,, 163 | ,,, 164 | ,,, 165 | ,,, 166 | ,,, 167 | ,,, 168 | ,,, 169 | ,,, 170 | ,,, 171 | ,,, 172 | ,,, 173 | ,,, 174 | ,,, 175 | ,,, 176 | ,,, 177 | ,,, 178 | ,,, 179 | ,,, 180 | ,,, 181 | ,,, 182 | ,,, 183 | ,,, 184 | ,,, 185 | ,,, 186 | ,,, 187 | ,,, 188 | ,,, 189 | ,,, 190 | ,,, 191 | ,,, 192 | ,,, 193 | ,,, 194 | ,,, 195 | ,,, 196 | ,,, 197 | ,,, 198 | ,,, 199 | ,,, 200 | ,,, 201 | ,,, 202 | ,,, 203 | ,,, 204 | ,,, 205 | ,,, 206 | ,,, 207 | ,,, 208 | ,,, 209 | ,,, 210 | ,,, 211 | ,,, 212 | ,,, 213 | ,,, 214 | ,,, 215 | ,,, 216 | ,,, 217 | ,,, 218 | ,,, 219 | ,,, 220 | ,,, 221 | ,,, 222 | ,,, 223 | ,,, 224 | ,,, 225 | ,,, 226 | ,,, 227 | ,,, 228 | ,,, 229 | ,,, 230 | ,,, 231 | ,,, 232 | ,,, 233 | ,,, 234 | ,,, 235 | ,,, 236 | ,,, 237 | ,,, 238 | ,,, 239 | ,,, 240 | ,,, 241 | ,,, 242 | ,,, 243 | ,,, 244 | ,,, 245 | ,,, 246 | ,,, 247 | ,,, 248 | ,,, 249 | ,,, 250 | ,,, 251 | ,,, 252 | ,,, 253 | ,,, 254 | ,,, 255 | ,,, 256 | ,,, 257 | ,,, 258 | ,,, 259 | //OUT 260 | ,,, 261 | ,,, 262 | ,,, 263 | ,,, 264 | ,,, 265 | ,,, 266 | ,,, 267 | ,,, 268 | ,,, 269 | ,,, 270 | ,,, 271 | ,,, 272 | ,,, 273 | ,,, 274 | ,,, 275 | ,,, 276 | ,,, 277 | ,,, 278 | ,,, 279 | ,,, 280 | ,,, 281 | ,,, 282 | ,,, 283 | ,,, 284 | ,,, 285 | ,,, 286 | ,,, 287 | ,,, 288 | ,,, 289 | ,,, 290 | ,,, 291 | ,,, 292 | ,,, 293 | ,,, 294 | ,,, 295 | ,,, 296 | ,,, 297 | ,,, 298 | ,,, 299 | ,,, 300 | ,,, 301 | ,,, 302 | ,,, 303 | ,,, 304 | ,,, 305 | ,,, 306 | ,,, 307 | ,,, 308 | ,,, 309 | ,,, 310 | ,,, 311 | ,,, 312 | ,,, 313 | ,,, 314 | ,,, 315 | ,,, 316 | ,,, 317 | ,,, 318 | ,,, 319 | ,,, 320 | ,,, 321 | ,,, 322 | ,,, 323 | ,,, 324 | ,,, 325 | ,,, 326 | ,,, 327 | ,,, 328 | ,,, 329 | ,,, 330 | ,,, 331 | ,,, 332 | ,,, 333 | ,,, 334 | ,,, 335 | ,,, 336 | ,,, 337 | ,,, 338 | ,,, 339 | ,,, 340 | ,,, 341 | ,,, 342 | ,,, 343 | ,,, 344 | ,,, 345 | ,,, 346 | ,,, 347 | ,,, 348 | ,,, 349 | ,,, 350 | ,,, 351 | ,,, 352 | ,,, 353 | ,,, 354 | ,,, 355 | ,,, 356 | ,,, 357 | ,,, 358 | ,,, 359 | ,,, 360 | ,,, 361 | ,,, 362 | ,,, 363 | ,,, 364 | ,,, 365 | ,,, 366 | ,,, 367 | ,,, 368 | ,,, 369 | ,,, 370 | ,,, 371 | ,,, 372 | ,,, 373 | ,,, 374 | ,,, 375 | ,,, 376 | ,,, 377 | ,,, 378 | ,,, 379 | ,,, 380 | ,,, 381 | ,,, 382 | ,,, 383 | ,,, 384 | ,,, 385 | ,,, 386 | ,,, 387 | ,,, 388 | ,,, 389 | ,,, 390 | ,,, 391 | ,,, 392 | ,,, 393 | ,,, 394 | ,,, 395 | ,,, 396 | ,,, 397 | ,,, 398 | ,,, 399 | ,,, 400 | ,,, 401 | ,,, 402 | ,,, 403 | ,,, 404 | ,,, 405 | ,,, 406 | ,,, 407 | ,,, 408 | ,,, 409 | ,,, 410 | ,,, 411 | ,,, 412 | ,,, 413 | ,,, 414 | ,,, 415 | ,,, 416 | ,,, 417 | ,,, 418 | ,,, 419 | ,,, 420 | ,,, 421 | ,,, 422 | ,,, 423 | ,,, 424 | ,,, 425 | ,,, 426 | ,,, 427 | ,,, 428 | ,,, 429 | ,,, 430 | ,,, 431 | ,,, 432 | ,,, 433 | ,,, 434 | ,,, 435 | ,,, 436 | ,,, 437 | ,,, 438 | ,,, 439 | ,,, 440 | ,,, 441 | ,,, 442 | ,,, 443 | ,,, 444 | ,,, 445 | ,,, 446 | ,,, 447 | ,,, 448 | ,,, 449 | ,,, 450 | ,,, 451 | ,,, 452 | ,,, 453 | ,,, 454 | ,,, 455 | ,,, 456 | ,,, 457 | ,,, 458 | ,,, 459 | ,,, 460 | ,,, 461 | ,,, 462 | ,,, 463 | ,,, 464 | ,,, 465 | ,,, 466 | ,,, 467 | ,,, 468 | ,,, 469 | ,,, 470 | ,,, 471 | ,,, 472 | ,,, 473 | ,,, 474 | ,,, 475 | ,,, 476 | ,,, 477 | ,,, 478 | ,,, 479 | ,,, 480 | ,,, 481 | ,,, 482 | ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV 483 | MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE 484 | ,,, 485 | ,,, 486 | ,,, 487 | ,,, 488 | ,,, 489 | ,,, 490 | ,,, 491 | ,,, 492 | ,,, 493 | ,,, 494 | ,,, 495 | ,,, 496 | ,,, 497 | ,,, 498 | ,,, 499 | ,,, 500 | ,,, 501 | ,,, 502 | ,,, 503 | ,,, 504 | ,,, 505 | ,,, 506 | ,,, 507 | ,,, 508 | ,,, 509 | ,,, 510 | ,,, 511 | ,,, 512 | ,,, 513 | ,,, 514 | ,,, 515 | ,,, 516 | -------------------------------------------------------------------------------- /robot_jobs/single_arm_with_ext_axis/INIT_ROS.JBI: -------------------------------------------------------------------------------- 1 | /JOB 2 | //NAME INIT_ROS 3 | //POS 4 | ///NPOS 0,0,0,0,0,0 5 | //INST 6 | ///DATE 2012/01/01 06:53 7 | ///ATTR SC,RW 8 | ///GROUP1 RB1 9 | ///GROUP2 ST1 10 | NOP 11 | DOUT OT#(890) OFF 12 | DOUT OT#(889) OFF 13 | TIMER T=0.05 14 | DOUT OT#(889) ON 15 | WAIT OT#(890)=ON 16 | DOUT OT#(890) OFF 17 | DOUT OT#(889) OFF 18 | END 19 | -------------------------------------------------------------------------------- /robot_jobs/single_arm_with_ext_axis/IONAME.DAT: -------------------------------------------------------------------------------- 1 | /IONAME 2 | //IN 3 | ,,, 4 | ,,, 5 | ,,, 6 | ,,, 7 | ,,, 8 | ,,, 9 | ,,, 10 | ,,, 11 | ,,, 12 | ,,, 13 | ,,, 14 | ,,, 15 | ,,, 16 | ,,, 17 | ,,, 18 | ,,, 19 | ,,, 20 | ,,, 21 | ,,, 22 | ,,, 23 | ,,, 24 | ,,, 25 | ,,, 26 | ,,, 27 | ,,, 28 | ,,, 29 | ,,, 30 | ,,, 31 | ,,, 32 | ,,, 33 | ,,, 34 | ,,, 35 | ,,, 36 | ,,, 37 | ,,, 38 | ,,, 39 | ,,, 40 | ,,, 41 | ,,, 42 | ,,, 43 | ,,, 44 | ,,, 45 | ,,, 46 | ,,, 47 | ,,, 48 | ,,, 49 | ,,, 50 | ,,, 51 | ,,, 52 | ,,, 53 | ,,, 54 | ,,, 55 | ,,, 56 | ,,, 57 | ,,, 58 | ,,, 59 | ,,, 60 | ,,, 61 | ,,, 62 | ,,, 63 | ,,, 64 | ,,, 65 | ,,, 66 | ,,, 67 | ,,, 68 | ,,, 69 | ,,, 70 | ,,, 71 | ,,, 72 | ,,, 73 | ,,, 74 | ,,, 75 | ,,, 76 | ,,, 77 | ,,, 78 | ,,, 79 | ,,, 80 | ,,, 81 | ,,, 82 | ,,, 83 | ,,, 84 | ,,, 85 | ,,, 86 | ,,, 87 | ,,, 88 | ,,, 89 | ,,, 90 | ,,, 91 | ,,, 92 | ,,, 93 | ,,, 94 | ,,, 95 | ,,, 96 | ,,, 97 | ,,, 98 | ,,, 99 | ,,, 100 | ,,, 101 | ,,, 102 | ,,, 103 | ,,, 104 | ,,, 105 | ,,, 106 | ,,, 107 | ,,, 108 | ,,, 109 | ,,, 110 | ,,, 111 | ,,, 112 | ,,, 113 | ,,, 114 | ,,, 115 | ,,, 116 | ,,, 117 | ,,, 118 | ,,, 119 | ,,, 120 | ,,, 121 | ,,, 122 | ,,, 123 | ,,, 124 | ,,, 125 | ,,, 126 | ,,, 127 | ,,, 128 | ,,, 129 | ,,, 130 | ,,, 131 | ,,, 132 | ,,, 133 | ,,, 134 | ,,, 135 | ,,, 136 | ,,, 137 | ,,, 138 | ,,, 139 | ,,, 140 | ,,, 141 | ,,, 142 | ,,, 143 | ,,, 144 | ,,, 145 | ,,, 146 | ,,, 147 | ,,, 148 | ,,, 149 | ,,, 150 | ,,, 151 | ,,, 152 | ,,, 153 | ,,, 154 | ,,, 155 | ,,, 156 | ,,, 157 | ,,, 158 | ,,, 159 | ,,, 160 | ,,, 161 | ,,, 162 | ,,, 163 | ,,, 164 | ,,, 165 | ,,, 166 | ,,, 167 | ,,, 168 | ,,, 169 | ,,, 170 | ,,, 171 | ,,, 172 | ,,, 173 | ,,, 174 | ,,, 175 | ,,, 176 | ,,, 177 | ,,, 178 | ,,, 179 | ,,, 180 | ,,, 181 | ,,, 182 | ,,, 183 | ,,, 184 | ,,, 185 | ,,, 186 | ,,, 187 | ,,, 188 | ,,, 189 | ,,, 190 | ,,, 191 | ,,, 192 | ,,, 193 | ,,, 194 | ,,, 195 | ,,, 196 | ,,, 197 | ,,, 198 | ,,, 199 | ,,, 200 | ,,, 201 | ,,, 202 | ,,, 203 | ,,, 204 | ,,, 205 | ,,, 206 | ,,, 207 | ,,, 208 | ,,, 209 | ,,, 210 | ,,, 211 | ,,, 212 | ,,, 213 | ,,, 214 | ,,, 215 | ,,, 216 | ,,, 217 | ,,, 218 | ,,, 219 | ,,, 220 | ,,, 221 | ,,, 222 | ,,, 223 | ,,, 224 | ,,, 225 | ,,, 226 | ,,, 227 | ,,, 228 | ,,, 229 | ,,, 230 | ,,, 231 | ,,, 232 | ,,, 233 | ,,, 234 | ,,, 235 | ,,, 236 | ,,, 237 | ,,, 238 | ,,, 239 | ,,, 240 | ,,, 241 | ,,, 242 | ,,, 243 | ,,, 244 | ,,, 245 | ,,, 246 | ,,, 247 | ,,, 248 | ,,, 249 | ,,, 250 | ,,, 251 | ,,, 252 | ,,, 253 | ,,, 254 | ,,, 255 | ,,, 256 | ,,, 257 | ,,, 258 | ,,, 259 | //OUT 260 | ,,, 261 | ,,, 262 | ,,, 263 | ,,, 264 | ,,, 265 | ,,, 266 | ,,, 267 | ,,, 268 | ,,, 269 | ,,, 270 | ,,, 271 | ,,, 272 | ,,, 273 | ,,, 274 | ,,, 275 | ,,, 276 | ,,, 277 | ,,, 278 | ,,, 279 | ,,, 280 | ,,, 281 | ,,, 282 | ,,, 283 | ,,, 284 | ,,, 285 | ,,, 286 | ,,, 287 | ,,, 288 | ,,, 289 | ,,, 290 | ,,, 291 | ,,, 292 | ,,, 293 | ,,, 294 | ,,, 295 | ,,, 296 | ,,, 297 | ,,, 298 | ,,, 299 | ,,, 300 | ,,, 301 | ,,, 302 | ,,, 303 | ,,, 304 | ,,, 305 | ,,, 306 | ,,, 307 | ,,, 308 | ,,, 309 | ,,, 310 | ,,, 311 | ,,, 312 | ,,, 313 | ,,, 314 | ,,, 315 | ,,, 316 | ,,, 317 | ,,, 318 | ,,, 319 | ,,, 320 | ,,, 321 | ,,, 322 | ,,, 323 | ,,, 324 | ,,, 325 | ,,, 326 | ,,, 327 | ,,, 328 | ,,, 329 | ,,, 330 | ,,, 331 | ,,, 332 | ,,, 333 | ,,, 334 | ,,, 335 | ,,, 336 | ,,, 337 | ,,, 338 | ,,, 339 | ,,, 340 | ,,, 341 | ,,, 342 | ,,, 343 | ,,, 344 | ,,, 345 | ,,, 346 | ,,, 347 | ,,, 348 | ,,, 349 | ,,, 350 | ,,, 351 | ,,, 352 | ,,, 353 | ,,, 354 | ,,, 355 | ,,, 356 | ,,, 357 | ,,, 358 | ,,, 359 | ,,, 360 | ,,, 361 | ,,, 362 | ,,, 363 | ,,, 364 | ,,, 365 | ,,, 366 | ,,, 367 | ,,, 368 | ,,, 369 | ,,, 370 | ,,, 371 | ,,, 372 | ,,, 373 | ,,, 374 | ,,, 375 | ,,, 376 | ,,, 377 | ,,, 378 | ,,, 379 | ,,, 380 | ,,, 381 | ,,, 382 | ,,, 383 | ,,, 384 | ,,, 385 | ,,, 386 | ,,, 387 | ,,, 388 | ,,, 389 | ,,, 390 | ,,, 391 | ,,, 392 | ,,, 393 | ,,, 394 | ,,, 395 | ,,, 396 | ,,, 397 | ,,, 398 | ,,, 399 | ,,, 400 | ,,, 401 | ,,, 402 | ,,, 403 | ,,, 404 | ,,, 405 | ,,, 406 | ,,, 407 | ,,, 408 | ,,, 409 | ,,, 410 | ,,, 411 | ,,, 412 | ,,, 413 | ,,, 414 | ,,, 415 | ,,, 416 | ,,, 417 | ,,, 418 | ,,, 419 | ,,, 420 | ,,, 421 | ,,, 422 | ,,, 423 | ,,, 424 | ,,, 425 | ,,, 426 | ,,, 427 | ,,, 428 | ,,, 429 | ,,, 430 | ,,, 431 | ,,, 432 | ,,, 433 | ,,, 434 | ,,, 435 | ,,, 436 | ,,, 437 | ,,, 438 | ,,, 439 | ,,, 440 | ,,, 441 | ,,, 442 | ,,, 443 | ,,, 444 | ,,, 445 | ,,, 446 | ,,, 447 | ,,, 448 | ,,, 449 | ,,, 450 | ,,, 451 | ,,, 452 | ,,, 453 | ,,, 454 | ,,, 455 | ,,, 456 | ,,, 457 | ,,, 458 | ,,, 459 | ,,, 460 | ,,, 461 | ,,, 462 | ,,, 463 | ,,, 464 | ,,, 465 | ,,, 466 | ,,, 467 | ,,, 468 | ,,, 469 | ,,, 470 | ,,, 471 | ,,, 472 | ,,, 473 | ,,, 474 | ,,, 475 | ,,, 476 | ,,, 477 | ,,, 478 | ,,, 479 | ,,, 480 | ,,, 481 | ,,, 482 | ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV 483 | MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE 484 | ,,, 485 | ,,, 486 | ,,, 487 | ,,, 488 | ,,, 489 | ,,, 490 | ,,, 491 | ,,, 492 | ,,, 493 | ,,, 494 | ,,, 495 | ,,, 496 | ,,, 497 | ,,, 498 | ,,, 499 | ,,, 500 | ,,, 501 | ,,, 502 | ,,, 503 | ,,, 504 | ,,, 505 | ,,, 506 | ,,, 507 | ,,, 508 | ,,, 509 | ,,, 510 | ,,, 511 | ,,, 512 | ,,, 513 | ,,, 514 | ,,, 515 | ,,, 516 | -------------------------------------------------------------------------------- /robot_jobs/two_arms/INIT_ROS.JBI: -------------------------------------------------------------------------------- 1 | /JOB 2 | //NAME INIT_ROS 3 | //POS 4 | ///NPOS 0,0,0,0,0,0 5 | //INST 6 | ///DATE 2012/01/01 06:53 7 | ///ATTR SC,RW 8 | ///GROUP1 RB1 9 | ///GROUP2 RB2 10 | NOP 11 | DOUT OT#(890) OFF 12 | DOUT OT#(889) OFF 13 | TIMER T=0.05 14 | DOUT OT#(889) ON 15 | WAIT OT#(890)=ON 16 | DOUT OT#(890) OFF 17 | DOUT OT#(889) OFF 18 | END 19 | -------------------------------------------------------------------------------- /robot_jobs/two_arms/IONAME.DAT: -------------------------------------------------------------------------------- 1 | /IONAME 2 | //IN 3 | ,,, 4 | ,,, 5 | ,,, 6 | ,,, 7 | ,,, 8 | ,,, 9 | ,,, 10 | ,,, 11 | ,,, 12 | ,,, 13 | ,,, 14 | ,,, 15 | ,,, 16 | ,,, 17 | ,,, 18 | ,,, 19 | ,,, 20 | ,,, 21 | ,,, 22 | ,,, 23 | ,,, 24 | ,,, 25 | ,,, 26 | ,,, 27 | ,,, 28 | ,,, 29 | ,,, 30 | ,,, 31 | ,,, 32 | ,,, 33 | ,,, 34 | ,,, 35 | ,,, 36 | ,,, 37 | ,,, 38 | ,,, 39 | ,,, 40 | ,,, 41 | ,,, 42 | ,,, 43 | ,,, 44 | ,,, 45 | ,,, 46 | ,,, 47 | ,,, 48 | ,,, 49 | ,,, 50 | ,,, 51 | ,,, 52 | ,,, 53 | ,,, 54 | ,,, 55 | ,,, 56 | ,,, 57 | ,,, 58 | ,,, 59 | ,,, 60 | ,,, 61 | ,,, 62 | ,,, 63 | ,,, 64 | ,,, 65 | ,,, 66 | ,,, 67 | ,,, 68 | ,,, 69 | ,,, 70 | ,,, 71 | ,,, 72 | ,,, 73 | ,,, 74 | ,,, 75 | ,,, 76 | ,,, 77 | ,,, 78 | ,,, 79 | ,,, 80 | ,,, 81 | ,,, 82 | ,,, 83 | ,,, 84 | ,,, 85 | ,,, 86 | ,,, 87 | ,,, 88 | ,,, 89 | ,,, 90 | ,,, 91 | ,,, 92 | ,,, 93 | ,,, 94 | ,,, 95 | ,,, 96 | ,,, 97 | ,,, 98 | ,,, 99 | ,,, 100 | ,,, 101 | ,,, 102 | ,,, 103 | ,,, 104 | ,,, 105 | ,,, 106 | ,,, 107 | ,,, 108 | ,,, 109 | ,,, 110 | ,,, 111 | ,,, 112 | ,,, 113 | ,,, 114 | ,,, 115 | ,,, 116 | ,,, 117 | ,,, 118 | ,,, 119 | ,,, 120 | ,,, 121 | ,,, 122 | ,,, 123 | ,,, 124 | ,,, 125 | ,,, 126 | ,,, 127 | ,,, 128 | ,,, 129 | ,,, 130 | ,,, 131 | ,,, 132 | ,,, 133 | ,,, 134 | ,,, 135 | ,,, 136 | ,,, 137 | ,,, 138 | ,,, 139 | ,,, 140 | ,,, 141 | ,,, 142 | ,,, 143 | ,,, 144 | ,,, 145 | ,,, 146 | ,,, 147 | ,,, 148 | ,,, 149 | ,,, 150 | ,,, 151 | ,,, 152 | ,,, 153 | ,,, 154 | ,,, 155 | ,,, 156 | ,,, 157 | ,,, 158 | ,,, 159 | ,,, 160 | ,,, 161 | ,,, 162 | ,,, 163 | ,,, 164 | ,,, 165 | ,,, 166 | ,,, 167 | ,,, 168 | ,,, 169 | ,,, 170 | ,,, 171 | ,,, 172 | ,,, 173 | ,,, 174 | ,,, 175 | ,,, 176 | ,,, 177 | ,,, 178 | ,,, 179 | ,,, 180 | ,,, 181 | ,,, 182 | ,,, 183 | ,,, 184 | ,,, 185 | ,,, 186 | ,,, 187 | ,,, 188 | ,,, 189 | ,,, 190 | ,,, 191 | ,,, 192 | ,,, 193 | ,,, 194 | ,,, 195 | ,,, 196 | ,,, 197 | ,,, 198 | ,,, 199 | ,,, 200 | ,,, 201 | ,,, 202 | ,,, 203 | ,,, 204 | ,,, 205 | ,,, 206 | ,,, 207 | ,,, 208 | ,,, 209 | ,,, 210 | ,,, 211 | ,,, 212 | ,,, 213 | ,,, 214 | ,,, 215 | ,,, 216 | ,,, 217 | ,,, 218 | ,,, 219 | ,,, 220 | ,,, 221 | ,,, 222 | ,,, 223 | ,,, 224 | ,,, 225 | ,,, 226 | ,,, 227 | ,,, 228 | ,,, 229 | ,,, 230 | ,,, 231 | ,,, 232 | ,,, 233 | ,,, 234 | ,,, 235 | ,,, 236 | ,,, 237 | ,,, 238 | ,,, 239 | ,,, 240 | ,,, 241 | ,,, 242 | ,,, 243 | ,,, 244 | ,,, 245 | ,,, 246 | ,,, 247 | ,,, 248 | ,,, 249 | ,,, 250 | ,,, 251 | ,,, 252 | ,,, 253 | ,,, 254 | ,,, 255 | ,,, 256 | ,,, 257 | ,,, 258 | ,,, 259 | //OUT 260 | ,,, 261 | ,,, 262 | ,,, 263 | ,,, 264 | ,,, 265 | ,,, 266 | ,,, 267 | ,,, 268 | ,,, 269 | ,,, 270 | ,,, 271 | ,,, 272 | ,,, 273 | ,,, 274 | ,,, 275 | ,,, 276 | ,,, 277 | ,,, 278 | ,,, 279 | ,,, 280 | ,,, 281 | ,,, 282 | ,,, 283 | ,,, 284 | ,,, 285 | ,,, 286 | ,,, 287 | ,,, 288 | ,,, 289 | ,,, 290 | ,,, 291 | ,,, 292 | ,,, 293 | ,,, 294 | ,,, 295 | ,,, 296 | ,,, 297 | ,,, 298 | ,,, 299 | ,,, 300 | ,,, 301 | ,,, 302 | ,,, 303 | ,,, 304 | ,,, 305 | ,,, 306 | ,,, 307 | ,,, 308 | ,,, 309 | ,,, 310 | ,,, 311 | ,,, 312 | ,,, 313 | ,,, 314 | ,,, 315 | ,,, 316 | ,,, 317 | ,,, 318 | ,,, 319 | ,,, 320 | ,,, 321 | ,,, 322 | ,,, 323 | ,,, 324 | ,,, 325 | ,,, 326 | ,,, 327 | ,,, 328 | ,,, 329 | ,,, 330 | ,,, 331 | ,,, 332 | ,,, 333 | ,,, 334 | ,,, 335 | ,,, 336 | ,,, 337 | ,,, 338 | ,,, 339 | ,,, 340 | ,,, 341 | ,,, 342 | ,,, 343 | ,,, 344 | ,,, 345 | ,,, 346 | ,,, 347 | ,,, 348 | ,,, 349 | ,,, 350 | ,,, 351 | ,,, 352 | ,,, 353 | ,,, 354 | ,,, 355 | ,,, 356 | ,,, 357 | ,,, 358 | ,,, 359 | ,,, 360 | ,,, 361 | ,,, 362 | ,,, 363 | ,,, 364 | ,,, 365 | ,,, 366 | ,,, 367 | ,,, 368 | ,,, 369 | ,,, 370 | ,,, 371 | ,,, 372 | ,,, 373 | ,,, 374 | ,,, 375 | ,,, 376 | ,,, 377 | ,,, 378 | ,,, 379 | ,,, 380 | ,,, 381 | ,,, 382 | ,,, 383 | ,,, 384 | ,,, 385 | ,,, 386 | ,,, 387 | ,,, 388 | ,,, 389 | ,,, 390 | ,,, 391 | ,,, 392 | ,,, 393 | ,,, 394 | ,,, 395 | ,,, 396 | ,,, 397 | ,,, 398 | ,,, 399 | ,,, 400 | ,,, 401 | ,,, 402 | ,,, 403 | ,,, 404 | ,,, 405 | ,,, 406 | ,,, 407 | ,,, 408 | ,,, 409 | ,,, 410 | ,,, 411 | ,,, 412 | ,,, 413 | ,,, 414 | ,,, 415 | ,,, 416 | ,,, 417 | ,,, 418 | ,,, 419 | ,,, 420 | ,,, 421 | ,,, 422 | ,,, 423 | ,,, 424 | ,,, 425 | ,,, 426 | ,,, 427 | ,,, 428 | ,,, 429 | ,,, 430 | ,,, 431 | ,,, 432 | ,,, 433 | ,,, 434 | ,,, 435 | ,,, 436 | ,,, 437 | ,,, 438 | ,,, 439 | ,,, 440 | ,,, 441 | ,,, 442 | ,,, 443 | ,,, 444 | ,,, 445 | ,,, 446 | ,,, 447 | ,,, 448 | ,,, 449 | ,,, 450 | ,,, 451 | ,,, 452 | ,,, 453 | ,,, 454 | ,,, 455 | ,,, 456 | ,,, 457 | ,,, 458 | ,,, 459 | ,,, 460 | ,,, 461 | ,,, 462 | ,,, 463 | ,,, 464 | ,,, 465 | ,,, 466 | ,,, 467 | ,,, 468 | ,,, 469 | ,,, 470 | ,,, 471 | ,,, 472 | ,,, 473 | ,,, 474 | ,,, 475 | ,,, 476 | ,,, 477 | ,,, 478 | ,,, 479 | ,,, 480 | ,,, 481 | ,,, 482 | ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV 483 | MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE 484 | ,,, 485 | ,,, 486 | ,,, 487 | ,,, 488 | ,,, 489 | ,,, 490 | ,,, 491 | ,,, 492 | ,,, 493 | ,,, 494 | ,,, 495 | ,,, 496 | ,,, 497 | ,,, 498 | ,,, 499 | ,,, 500 | ,,, 501 | ,,, 502 | ,,, 503 | ,,, 504 | ,,, 505 | ,,, 506 | ,,, 507 | ,,, 508 | ,,, 509 | ,,, 510 | ,,, 511 | ,,, 512 | ,,, 513 | ,,, 514 | ,,, 515 | ,,, 516 | -------------------------------------------------------------------------------- /src/ActionServer_FJT.h: -------------------------------------------------------------------------------- 1 | // ActionServer_FJT.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_ACTION_SERVER_FJT_H 9 | #define MOTOROS2_ACTION_SERVER_FJT_H 10 | 11 | #define MAX_NUMBER_OF_POINTS_PER_TRAJECTORY 200 12 | #define MIN_NUMBER_OF_POINTS_PER_TRAJECTORY 2 //current position and destination 13 | 14 | #define DEFAULT_FJT_GOAL_POSITION_TOLERANCE (0.01) //radians per axis or meters per axis 15 | #define DEFAULT_FJT_GOAL_TIME_TOLERANCE (500000000LL) //nanoseconds (0.5 seconds) 16 | 17 | extern rclc_action_server_t g_actionServerFollowJointTrajectory; 18 | 19 | extern control_msgs__action__FollowJointTrajectory_SendGoal_Request g_actionServer_FJT_SendGoal_Request; 20 | extern UINT32 g_actionServer_FJT_SendGoal_Request__sizeof; 21 | 22 | extern void Ros_ActionServer_FJT_Initialize(); 23 | extern void Ros_ActionServer_FJT_Cleanup(); 24 | 25 | extern rcl_ret_t Ros_ActionServer_FJT_Goal_Received(rclc_action_goal_handle_t* goal_handle, void* context); 26 | extern void Ros_ActionServer_FJT_ProcessFeedback(); 27 | extern void Ros_ActionServer_FJT_ProcessResult(); 28 | extern bool Ros_ActionServer_FJT_Goal_Cancel(rclc_action_goal_handle_t* goal_handle, void* context); 29 | 30 | extern void Ros_ActionServer_FJT_UpdateProgressTracker(MP_EXPOS_DATA* incrementData); 31 | 32 | 33 | #endif // MOTOROS2_ACTION_SERVER_FJT_H 34 | -------------------------------------------------------------------------------- /src/CommunicationExecutor.h: -------------------------------------------------------------------------------- 1 | //Communication.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_COMMUNICATION_EXECUTOR_H 9 | #define MOTOROS2_COMMUNICATION_EXECUTOR_H 10 | 11 | #define PERIOD_COMMUNICATION_PING_AGENT_MS 5000 12 | 13 | #define PERIOD_COMMUNICATION_USERLAN_LINK_CHECK_MS 500 14 | 15 | // total number of handles = 16 | // timers + 2 17 | // action_server + 1 18 | // service reset 1 19 | // service start_traj_mode 1 20 | // service start_point_queue_mode 1 21 | // service stop_traj_mode 1 22 | // service queue_traj_point 1 23 | // service select_tool 1 24 | #define QUANTITY_OF_HANDLES_FOR_MOTION_EXECUTOR (9) 25 | 26 | // total number of handles = 27 | // timers + 1 28 | // service read & write I/O + 6 29 | #define QUANTITY_OF_HANDLES_FOR_IO_EXECUTOR (7) 30 | 31 | typedef struct 32 | { 33 | rcl_init_options_t initOptions; 34 | rclc_support_t support; 35 | rcl_node_t node; 36 | } Communication_NodeInfo; 37 | extern Communication_NodeInfo g_microRosNodeInfo; 38 | 39 | extern BOOL g_Ros_Communication_AgentIsConnected; 40 | 41 | extern void Ros_Communication_ConnectToAgent(); 42 | extern void Ros_Communication_StartExecutors(SEM_ID semCommunicationExecutorStatus); 43 | 44 | extern void Ros_Communication_Initialize(); 45 | extern void Ros_Communication_Cleanup(); 46 | 47 | #endif // MOTOROS2_COMMUNICATION_EXECUTOR_H 48 | -------------------------------------------------------------------------------- /src/ConfigFile.h: -------------------------------------------------------------------------------- 1 | // ConfigFile.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_CONFIG_FILE_H 9 | #define MOTOROS2_CONFIG_FILE_H 10 | 11 | //For ROS_USER_LAN1 and ROS_USER_LAN2 12 | #include "MotoROS_PlatformLib.h" 13 | 14 | 15 | #define CONFIG_FILE_NAME "motoros2_config.yaml" 16 | #define FORMAT_CONFIG_FILE_BACKUP "%s\\%s.%04d%02d%02d_%02d%02d%02d" //Example: "MPUSB0\motoros2_config.yaml.202207029_081011" 17 | 18 | #define MAX_YAML_STRING_LEN 128 19 | 20 | #define DEFAULT_ROS_DOMAIN_ID 0 21 | #define MIN_ROS_DOMAIN_ID_LINUX 0 22 | #define MAX_ROS_DOMAIN_ID_LINUX 101 23 | 24 | #define PLACEHOLDER_NODE_NAME "" //temporary/placeholder 25 | #define DEFAULT_NODE_NAME_PREFIX "motoman" //will be suffixed with MAC ID 26 | 27 | #define DEFAULT_NODE_NAMESPACE "" 28 | 29 | #define DEFAULT_SYNCTIME TRUE 30 | 31 | #define DEFAULT_PUBLISH_TF TRUE 32 | 33 | #define DEFAULT_NAMESPACE_TF TRUE 34 | 35 | // NOTE: We do not prefix joints by the "motoman Grp ID" here, but use the generic 36 | // group & joint names instead to avoid the OEM-specific names. 37 | #define DEFAULT_JOINT_NAME_FMT_SINGLE "joint_%d" 38 | #define DEFAULT_JOINT_NAME_FMT_MULTI "group_%d/joint_%d" 39 | 40 | #define DEFAULT_LOG_TO_STDOUT FALSE 41 | 42 | #define DEFAULT_EXECUTOR_SLEEP_PERIOD 10 //ms 43 | #define MIN_EXECUTOR_SLEEP_PERIOD 1 44 | #define MAX_EXECUTOR_SLEEP_PERIOD 100 45 | 46 | #define DEFAULT_FEEDBACK_PUBLISH_PERIOD 20 //ms 47 | #define MIN_FEEDBACK_PUBLISH_PERIOD 1 48 | #define MAX_FEEDBACK_PUBLISH_PERIOD 100 49 | 50 | #define DEFAULT_CONTROLLER_IO_PERIOD 10 //ms 51 | #define MIN_CONTROLLER_IO_PERIOD 1 52 | #define MAX_CONTROLLER_IO_PERIOD 100 53 | 54 | #define DEFAULT_QOS_ROBOT_STATUS ROS_QOS_PROFILE_SENSOR_DATA 55 | 56 | #define DEFAULT_QOS_JOINT_STATES ROS_QOS_PROFILE_SENSOR_DATA 57 | 58 | #define DEFAULT_QOS_TF ROS_QOS_PROFILE_DEFAULT 59 | 60 | #define DEFAULT_TF_FRAME_PREFIX "" 61 | 62 | #define DEFAULT_STOP_MOTION_ON_DISCON TRUE 63 | 64 | #define DEFAULT_ALLOW_CUSTOM_INFORM FALSE 65 | 66 | #define DEFAULT_INFORM_JOB_NAME "INIT_ROS" 67 | 68 | // based on rmw/qos_profiles.h 69 | typedef enum 70 | { 71 | ROS_QOS_PROFILE_SENSOR_DATA = 1, 72 | ROS_QOS_PROFILE_PARAMETERS = 2, 73 | ROS_QOS_PROFILE_DEFAULT = 3, 74 | ROS_QOS_PROFILE_SERVICES = 4, 75 | ROS_QOS_PROFILE_PARAMETER_EVENTS = 5, 76 | ROS_QOS_PROFILE_SYSTEM_DEFAULT = 6, 77 | } Ros_QoS_Profile_Setting; 78 | 79 | #define ROS_QOS_PROFILE_SENSOR_DATA_NAME "sensor_data" 80 | #define ROS_QOS_PROFILE_PARAMETERS_NAME "parameters" 81 | #define ROS_QOS_PROFILE_DEFAULT_NAME "default" 82 | #define ROS_QOS_PROFILE_SERVICES_NAME "services" 83 | #define ROS_QOS_PROFILE_PARAMETER_EVENTS_NAME "parameter_events" 84 | #define ROS_QOS_PROFILE_SYSTEM_DEFAULT_NAME "system_default" 85 | #define ROS_QOS_PROFILE_UNKNOWN_NAME "unknown" 86 | 87 | #define DEFAULT_REMAP_RULES "" 88 | #define MAX_REMAP_RULE_NUM 16 89 | #define MAX_REMAP_RULE_LEN 256 90 | 91 | typedef enum 92 | { 93 | CFG_ROS_USER_LAN_MALFORMED = -3, //sentinel 94 | CFG_ROS_USER_LAN_ALL = -2, //sentinel 95 | CFG_ROS_USER_LAN_AUTO = -1, //sentinel 96 | CFG_ROS_USER_LAN1 = ROS_USER_LAN1, 97 | CFG_ROS_USER_LAN2 = ROS_USER_LAN2, 98 | } Ros_UserLan_Port_Setting; 99 | 100 | #define DEFAULT_ULAN_MON_ENABLED TRUE 101 | #define DEFAULT_ULAN_MON_LINK CFG_ROS_USER_LAN_AUTO 102 | 103 | #define DEFAULT_IGNORE_MISSING_CALIB FALSE 104 | 105 | #define DEFAULT_ULAN_DEBUG_BROADCAST_ENABLED TRUE 106 | 107 | #if defined (YRC1000) 108 | #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN_ALL 109 | #else 110 | #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 111 | #endif 112 | typedef struct 113 | { 114 | //TODO(gavanderhoorn): add support for unsigned types 115 | int ros_domain_id; 116 | 117 | char node_name[MAX_YAML_STRING_LEN]; 118 | char node_namespace[MAX_YAML_STRING_LEN]; 119 | 120 | char remap_rules[MAX_REMAP_RULE_LEN]; 121 | 122 | char agent_ip_address[MAX_YAML_STRING_LEN]; 123 | char agent_port_number[MAX_YAML_STRING_LEN]; 124 | 125 | BOOL sync_timeclock_with_agent; 126 | 127 | BOOL publish_tf; 128 | BOOL namespace_tf; 129 | 130 | char joint_names[MAX_CONTROLLABLE_GROUPS * MP_GRP_AXES_NUM][MAX_JOINT_NAME_LENGTH]; 131 | 132 | BOOL log_to_stdout; 133 | 134 | int executor_sleep_period; 135 | int action_feedback_publisher_period; 136 | int controller_status_monitor_period; 137 | 138 | Ros_QoS_Profile_Setting qos_robot_status; 139 | Ros_QoS_Profile_Setting qos_joint_states; 140 | Ros_QoS_Profile_Setting qos_tf; 141 | 142 | char tf_frame_prefix[MAX_YAML_STRING_LEN]; 143 | 144 | BOOL stop_motion_on_disconnect; 145 | 146 | char inform_job_name[MAX_JOB_NAME_LEN]; 147 | 148 | BOOL allow_custom_inform_job; 149 | 150 | BOOL userlan_monitor_enabled; 151 | Ros_UserLan_Port_Setting userlan_monitor_port; 152 | 153 | BOOL ignore_missing_calib_data; 154 | 155 | BOOL debug_broadcast_enabled; 156 | Ros_UserLan_Port_Setting debug_broadcast_port; 157 | } Ros_Configuration_Settings; 158 | 159 | extern Ros_Configuration_Settings g_nodeConfigSettings; 160 | 161 | extern void Ros_ConfigFile_SetAllDefaultValues(); 162 | 163 | extern void Ros_ConfigFile_Parse(); 164 | 165 | extern rmw_qos_profile_t const* const Ros_ConfigFile_To_Rmw_Qos_Profile(Ros_QoS_Profile_Setting val); 166 | 167 | #endif // MOTOROS2_CONFIG_FILE_H 168 | -------------------------------------------------------------------------------- /src/ControllerStatusIO.h: -------------------------------------------------------------------------------- 1 | //Controller.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_CONTROLLER_STATUS_IO_H 9 | #define MOTOROS2_CONTROLLER_STATUS_IO_H 10 | 11 | #define IO_FEEDBACK_WAITING_MP_INCMOVE 11120 //output# 889 12 | #define IO_FEEDBACK_MP_INCMOVE_DONE 11121 //output# 890 13 | #define IO_FEEDBACK_INITIALIZATION_DONE 11122 //output# 891 14 | #define IO_FEEDBACK_CONTROLLERRUNNING 11123 //output# 892 15 | #define IO_FEEDBACK_AGENTCONNECTED 11124 //output# 893 16 | #define IO_FEEDBACK__ 11125 //output# 894 17 | #define IO_FEEDBACK___ 11126 //output# 895 18 | #define IO_FEEDBACK_FAILURE 11127 //output# 896 19 | 20 | #define IO_FEEDBACK_RESERVED_1 11130 //output# 897 21 | #define IO_FEEDBACK_RESERVED_2 11131 //output# 898 22 | #define IO_FEEDBACK_RESERVED_3 11132 //output# 899 23 | #define IO_FEEDBACK_RESERVED_4 11133 //output# 900 24 | #define IO_FEEDBACK_RESERVED_5 11134 //output# 901 25 | #define IO_FEEDBACK_RESERVED_6 11135 //output# 902 26 | #define IO_FEEDBACK_RESERVED_7 11136 //output# 903 27 | #define IO_FEEDBACK_RESERVED_8 11137 //output# 904 28 | 29 | #define INVALID_TASK -1 30 | 31 | #define MAX_CONTROLLABLE_GROUPS 8 32 | 33 | #define MASK_ISALARM_ACTIVEALARM 0x02 34 | #define MASK_ISALARM_ACTIVEERROR 0x01 35 | 36 | #define MAX_ROBOT_CALIBRATION_FILES 32 37 | 38 | #define MIN_VALID_TOOL_INDEX 0 39 | #define MAX_VALID_TOOL_INDEX 63 40 | 41 | typedef enum 42 | { 43 | IO_ROBOTSTATUS_ALARM_MAJOR = 0, 44 | IO_ROBOTSTATUS_ALARM_MINOR, 45 | IO_ROBOTSTATUS_ALARM_SYSTEM, 46 | IO_ROBOTSTATUS_ALARM_USER, 47 | IO_ROBOTSTATUS_ERROR, 48 | IO_ROBOTSTATUS_PLAY, 49 | IO_ROBOTSTATUS_TEACH, 50 | IO_ROBOTSTATUS_REMOTE, 51 | IO_ROBOTSTATUS_OPERATING, 52 | IO_ROBOTSTATUS_HOLD, 53 | IO_ROBOTSTATUS_SERVO, 54 | IO_ROBOTSTATUS_ESTOP_EX, 55 | IO_ROBOTSTATUS_ESTOP_PP, 56 | IO_ROBOTSTATUS_ESTOP_CTRL, 57 | IO_ROBOTSTATUS_WAITING_ROS, 58 | IO_ROBOTSTATUS_INECOMODE, 59 | IO_ROBOTSTATUS_CONT_CYC_MODE, 60 | #if (YRC1000||YRC1000u) 61 | IO_ROBOTSTATUS_PFL_STOP, 62 | IO_ROBOTSTATUS_PFL_ESCAPE, 63 | IO_ROBOTSTATUS_PFL_AVOIDING, 64 | IO_ROBOTSTATUS_PFL_AVOID_JOINT, 65 | IO_ROBOTSTATUS_PFL_AVOID_TRANS, 66 | #endif 67 | IO_ROBOTSTATUS_MAX 68 | } IoStatusIndex; 69 | 70 | typedef struct 71 | { 72 | UINT16 interpolPeriod; // Interpolation period of the controller 73 | int numGroup; // Actual number of defined group 74 | //int numRobot; // Actual number of defined robot 75 | int totalAxesCount; // Number of axes attached to the controller (all groups) 76 | CtrlGroup* ctrlGroups[MAX_CONTROLLABLE_GROUPS]; // Array of the controller control group 77 | 78 | UINT8 rosTrafficLanPort; 79 | UCHAR rosTrafficMacAddr[6]; 80 | 81 | // Controller Status 82 | MP_IO_INFO ioStatusAddr[IO_ROBOTSTATUS_MAX]; // Array of Specific Input Address representing the I/O status 83 | USHORT ioStatus[IO_ROBOTSTATUS_MAX]; // Array storing the current status of the controller 84 | int alarmCode; // Alarm number currently active 85 | BOOL bStopMotion; // Flag to stop motion 86 | BOOL bPFLEnabled; // Flag indicating that the controller has the PFL option enabled 87 | BOOL bPFLduringRosMove; // Flag to keep track PFL activation during RosMotion 88 | BOOL bMpIncMoveError; // Flag indicating that the incremental motion API failed 89 | BOOL bPrevAlarmState; // Flag indicating if there was an active ALARM during the last I/O cycle 90 | 91 | int tidIncMoveThread; // ThreadId for sending the incremental move to the controller 92 | } Controller; 93 | 94 | typedef struct 95 | { 96 | rcl_publisher_t robotStatus; 97 | } ControllerStatus_Publishers; 98 | extern ControllerStatus_Publishers g_publishers_RobotStatus; 99 | 100 | typedef struct 101 | { 102 | industrial_msgs__msg__RobotStatus* msgRobotStatus; 103 | } ControllerStatus_Messages; 104 | extern ControllerStatus_Messages g_messages_RobotStatus; 105 | 106 | extern Controller g_Ros_Controller; 107 | 108 | extern BOOL Ros_Controller_Initialize(); 109 | extern void Ros_Controller_Cleanup(); 110 | 111 | extern BOOL Ros_Controller_IsValidGroupNo(int groupNo); 112 | 113 | extern void Ros_Controller_StatusInit(); 114 | extern BOOL Ros_Controller_StatusRead(USHORT ioStatus[IO_ROBOTSTATUS_MAX]); 115 | extern BOOL Ros_Controller_IoStatusUpdate(); 116 | extern BOOL Ros_Controller_IsAlarm(); 117 | extern BOOL Ros_Controller_IsMajorAlarm(); 118 | extern BOOL Ros_Controller_IsError(); 119 | extern BOOL Ros_Controller_IsPlay(); 120 | extern BOOL Ros_Controller_IsTeach(); 121 | extern BOOL Ros_Controller_IsRemote(); 122 | extern BOOL Ros_Controller_IsOperating(); 123 | extern BOOL Ros_Controller_IsHold(); 124 | extern BOOL Ros_Controller_IsServoOn(); 125 | extern BOOL Ros_Controller_IsEcoMode(); 126 | extern BOOL Ros_Controller_IsEStop(); 127 | extern BOOL Ros_Controller_IsWaitingRos(); 128 | extern BOOL Ros_Controller_IsContinuousCycle(); 129 | extern BOOL Ros_Controller_IsMotionReady(); 130 | extern BOOL Ros_Controller_IsInMotion(); 131 | extern BOOL Ros_Controller_IsPflActive(); 132 | extern BOOL Ros_Controller_IsMpIncMoveErrorActive(); 133 | extern BOOL Ros_Controller_IsAnyFaultActive(); 134 | extern BOOL Ros_Controller_MasterTaskIsJobName(const char* const jobName); 135 | extern MotionNotReadyCode Ros_Controller_GetNotReadySubcode(bool ignoreTractableProblems); 136 | 137 | //reset internal flag indicating whether PFL became active during a move 138 | extern void Ros_Controller_Reset_PflDuringRosMove(); 139 | 140 | //reset internal flag indicating whether mpIncMove(..) has errored 141 | extern void Ros_Controller_Reset_MpIncMoveError(); 142 | 143 | extern BOOL Ros_Controller_GetIOState(ULONG signal); 144 | extern void Ros_Controller_SetIOState(ULONG signal, BOOL status); 145 | 146 | extern int Ros_Controller_GetAlarmCode(); 147 | 148 | //retrieve all active alarms (and a possible error) and store them in 'active_alarms' 149 | extern int Ros_Controller_GetActiveAlarmCodes(USHORT active_alarms[MAX_ALARM_COUNT + MAX_ERROR_COUNT]); 150 | 151 | //TODO(gavanderhoorn): make static, see comment on definition 152 | extern BOOL Ros_Controller_ShouldWarnNoCalibDataLoaded(Controller const* controller, BOOL bCalibLoadedOk, BOOL bPublishTfEnabled); 153 | 154 | //#define DUMMY_SERVO_MODE 1 // Dummy servo mode is used for testing with Yaskawa debug controllers 155 | #ifdef DUMMY_SERVO_MODE 156 | #warning Dont forget to disable DUMMY_SERO_MODE when done testing 157 | #endif 158 | 159 | #endif // MOTOROS2_CONTROLLER_STATUS_IO_H 160 | -------------------------------------------------------------------------------- /src/CtrlGroup.h: -------------------------------------------------------------------------------- 1 | // CtrlGroup.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_CTRL_GROUP_H 9 | #define MOTOROS2_CTRL_GROUP_H 10 | 11 | 12 | #define Q_SIZE 200 13 | 14 | #if (DX100 || DX200 || FS100) 15 | #define Q_LOCK_TIMEOUT 1000 16 | #else 17 | #define Q_LOCK_TIMEOUT 5000 //YRC1000 tick period is 0.2 ms 18 | #endif 19 | 20 | #define Q_OFFSET_IDX( a, b, c ) (((a)+(b)) >= (c) ) ? ((a)+(b)-(c)) \ 21 | : ( (((a)+(b)) < 0 ) ? ((a)+(b)+(c)) : ((a)+(b)) ) 22 | 23 | #define MAX_JOINT_NAME_LENGTH 32 24 | #define MAX_TF_FRAME_NAME_LENGTH 96 25 | 26 | typedef struct 27 | { 28 | UINT64 time; 29 | UCHAR frame; 30 | UCHAR user; 31 | UCHAR tool; 32 | LONG inc[MP_GRP_AXES_NUM]; 33 | } Incremental_data; 34 | 35 | typedef struct 36 | { 37 | SEM_ID q_lock; 38 | LONG cnt; 39 | LONG idx; 40 | Incremental_data data[Q_SIZE]; 41 | } Incremental_q; 42 | 43 | // jointMotionData values are in radian and joint order in sequential order 44 | typedef struct 45 | { 46 | BOOL valid; // this point has valid data 47 | UINT64 time; // time in millisecond 48 | double pos[MP_GRP_AXES_NUM]; // position in radians 49 | double vel[MP_GRP_AXES_NUM]; // velocity in radians/s 50 | } JointMotionData; 51 | 52 | //--------------------------------------------------------------- 53 | // CtrlGroup: 54 | // Structure containing all the data related to a control group 55 | //--------------------------------------------------------------- 56 | typedef struct 57 | { 58 | int groupNo; // sequence group number 59 | int numAxes; // number of axis in the control group 60 | MP_GRP_ID_TYPE groupId; // control group ID 61 | PULSE_TO_RAD pulseToRad; // conversion ratio between pulse and radian 62 | PULSE_TO_METER pulseToMeter; // conversion ratio between pulse and meter (linear axis) 63 | FB_PULSE_CORRECTION_DATA correctionData; // compensation for axes coupling 64 | MAX_INCREMENT_INFO maxInc; // maximum increment per interpolation cycle 65 | double maxSpeed[MP_GRP_AXES_NUM]; // maximum joint speed in radian/sec (rotational) or meter/sec (linear) (ROS joint-order) 66 | int tool; // selected tool for the motion 67 | 68 | Incremental_q inc_q; // incremental queue 69 | UINT64 q_time; // time to which the queue has been processed 70 | 71 | JointMotionData* trajectoryIterator; // joint motion command data in radian 72 | JointMotionData* prevTrajectoryIterator; // joint motion command data in radian 73 | JointMotionData trajectoryToProcess[MAX_NUMBER_OF_POINTS_PER_TRAJECTORY]; // joint motion command data in radian to process 74 | 75 | BOOL hasDataToProcess; // indicates that there is data to process 76 | UINT64 timeLeftover_ms; // Time left over after reaching the end of a trajectory to complete the interpolation period 77 | long prevPulsePos[MAX_PULSE_AXES]; // The commanded pulse position that the trajectory starts at (Ros_MotionServer_StartTrajMode) 78 | AXIS_MOTION_TYPE axisType; // Indicates whether axis is rotary or linear 79 | char jointNames_userDefined[MP_GRP_AXES_NUM][MAX_JOINT_NAME_LENGTH]; //string name for each joint in 'moto' (non-sequential) joint order 80 | 81 | BOOL bIsBaxisSlave; // Indicates the B axis will automatically move to maintain orientation as other axes are moved 82 | 83 | MP_COORD robotCalibrationToBaseFrame; // Transform from [BF] > [RF] or [BF] > [base track origin] 84 | 85 | MP_GRP_ID_TYPE baseTrackGroupId; // ID for the base track associated with this robot (-1 if no base track) 86 | int baseTrackGroupIndex; // Group index for the base track associated with this robot (-1 if no base track) 87 | BASE_AXIS_INFO baseTrackInfo; // 88 | 89 | JOINT_FEEDBACK_SPEED_ADDRESSES speedFeedbackRegisterAddress; //CIO address for the registers containing feedback speed 90 | 91 | int tidAddToIncQueue; 92 | 93 | //------------------------------------------------------------------- 94 | //Publishers 95 | //------------------------------------------------------------------- 96 | rcl_publisher_t publisherJointState; 97 | sensor_msgs__msg__JointState* msgJointState; 98 | 99 | } CtrlGroup; 100 | 101 | 102 | //--------------------------------- 103 | // External Functions Declaration 104 | //--------------------------------- 105 | 106 | extern CtrlGroup* Ros_CtrlGroup_Ctor(); 107 | extern void Ros_CtrlGroup_Dtor(CtrlGroup* ctrlGroup); 108 | 109 | //Initialize specific control group. This should be called for each group connected to the robot 110 | //controller in numerical order. 111 | // int groupNo: Zero based index of the group number (0-3) 112 | // BOOL bIsLastGrpToInit: TRUE if this is the final group that is being initialized. FALSE if you plan to call this function again. 113 | // float interpolPeriod: Value of the interpolation period (ms) for the robot controller. 114 | extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod); 115 | extern void Ros_CtrlGrp_Cleanup(CtrlGroup* ctrlGroup); 116 | 117 | extern MP_GRP_ID_TYPE Ros_mpCtrlGrpNo2GrpId(int groupNo); 118 | 119 | extern BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]); 120 | extern BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]); 121 | extern BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES]); 122 | 123 | extern BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE_AXES]); 124 | 125 | extern BOOL Ros_CtrlGroup_GetEncoderTemperature(CtrlGroup const* const ctrlGroup, long encoderTemp[MAX_PULSE_AXES]); 126 | 127 | extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long const pulsePos[MAX_PULSE_AXES], double rosPos[MAX_PULSE_AXES]); 128 | extern void Ros_CtrlGroup_ConvertToRosTorque(CtrlGroup* ctrlGroup, double const motoTorque[MAX_PULSE_AXES], double rosTorque[MAX_PULSE_AXES]); 129 | extern void Ros_CtrlGroup_ConvertToMotoPos_FromSequentialOrdering(CtrlGroup* ctrlGroup, double const radPos[MAX_PULSE_AXES], long pulsePos[MAX_PULSE_AXES]); 130 | extern void Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(CtrlGroup* ctrlGroup, double const rosPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES]); 131 | 132 | extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup); 133 | 134 | extern BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup); 135 | extern BOOL Ros_CtrlGroup_IsBase(CtrlGroup const* const ctrlGroup); 136 | extern BOOL Ros_CtrlGroup_IsStation(CtrlGroup const* const ctrlGroup); 137 | extern BOOL Ros_CtrlGroup_IsInvalidAxis(CtrlGroup const* const ctrlGroup, size_t axisIdx); 138 | extern BOOL Ros_CtrlGroup_HasBaseTrack(CtrlGroup const* ctrlGroup); 139 | 140 | extern void Ros_CtrlGroup_UpdateJointNamesInMotoOrder(CtrlGroup* ctrlGroup); 141 | 142 | //String representation of MP_GRP_ID_TYPE enum. Use the MP_GRP_ID_TYPE enum as the index of this array. 143 | extern const char* Ros_CtrlGroup_GRP_ID_String[]; 144 | 145 | #endif // MOTOROS2_CTRL_GROUP_H 146 | -------------------------------------------------------------------------------- /src/DX200_foxyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/DX200_foxyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/DX200_galacticCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/DX200_galacticLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/DX200_humbleCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/DX200_humbleLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/DX200_jazzyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DDX200 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/DX200_jazzyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/Debug.c: -------------------------------------------------------------------------------- 1 | //Debug.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | 11 | #define DEBUG_UDP_PORT_NUMBER 21789 12 | 13 | #define MAX_DEBUG_MESSAGE_SIZE 1024 14 | 15 | #define FORMATTED_TIME_SIZE 1024 16 | 17 | typedef struct 18 | { 19 | UINT8 enabledPortCount; 20 | int debugSocket[MAX_NETWORK_PORTS]; 21 | struct sockaddr_in destAddr[MAX_NETWORK_PORTS]; 22 | } user_lan_info_t; 23 | 24 | user_lan_info_t ros_debugPorts = {0}; 25 | 26 | // This sets the host to broadcast the message on all ports. 27 | // This behavior will only be overwritten if the user specifies otherwise 28 | // in the config file. 29 | void Ros_Debug_Init() 30 | { 31 | ULONG ip_be; 32 | ULONG subnetmask_be; 33 | ULONG gateway_be; 34 | int broadcastVal = 1; 35 | UCHAR mac[6]; 36 | STATUS status; 37 | int count = 0; 38 | int* socket = ros_debugPorts.debugSocket; 39 | struct sockaddr_in* sin = ros_debugPorts.destAddr; 40 | 41 | bzero(&ros_debugPorts, sizeof(ros_debugPorts)); 42 | 43 | for (int i = 1; i <= MAX_NETWORK_PORTS; i++) 44 | { 45 | 46 | status = Ros_mpNICData(i, &ip_be, &subnetmask_be, mac, &gateway_be); 47 | 48 | if (status == OK) 49 | { 50 | socket[count] = mpSocket(AF_INET, SOCK_DGRAM, 0); 51 | Ros_setsockopt(socket[count], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal)); 52 | sin[count].sin_addr.s_addr = ip_be | (~subnetmask_be); 53 | sin[count].sin_family = AF_INET; 54 | sin[count].sin_port = mpHtons(DEBUG_UDP_PORT_NUMBER); 55 | count++; 56 | } 57 | } 58 | ros_debugPorts.enabledPortCount = count; 59 | } 60 | 61 | // This sets the host to broadcast the message only to the one port specified in the cfg file. 62 | // This overwrites the behavior from Ros_Debug_Init() so it will no longer broadcast 63 | // to all ports. 64 | void Ros_Debug_SetFromConfig() 65 | { 66 | ULONG ip_be; 67 | ULONG subnetmask_be; 68 | ULONG gateway_be; 69 | int broadcastVal = 1; 70 | UCHAR mac[6]; 71 | STATUS status; 72 | char message[ERROR_MSG_MAX_SIZE]; 73 | int* socket = ros_debugPorts.debugSocket; 74 | struct sockaddr_in* sin = ros_debugPorts.destAddr; 75 | 76 | bzero(&ros_debugPorts, sizeof(ros_debugPorts)); 77 | 78 | status = Ros_mpNICData(g_nodeConfigSettings.debug_broadcast_port, &ip_be, &subnetmask_be, mac, &gateway_be); 79 | 80 | // This function is only called if the user specifies which port the host should broadcast to. So 81 | // only one socket needs to have its information populated in this case. 82 | if (status == OK) 83 | { 84 | socket[0] = mpSocket(AF_INET, SOCK_DGRAM, 0); 85 | Ros_setsockopt(socket[0], SOL_SOCKET, SO_BROADCAST, (char*)&broadcastVal, sizeof(broadcastVal)); 86 | sin[0].sin_addr.s_addr = ip_be | (~subnetmask_be); 87 | sin[0].sin_family = AF_INET; 88 | sin[0].sin_port = mpHtons(DEBUG_UDP_PORT_NUMBER); 89 | ros_debugPorts.enabledPortCount = 1; 90 | } 91 | else 92 | { 93 | //If it fails, then disable debug broadcast and notify the user via an alarm. 94 | int ret = snprintf(message, ERROR_MSG_MAX_SIZE, "Enable LAN port %d for debug", g_nodeConfigSettings.debug_broadcast_port); 95 | if (0 < ret && ret <= ERROR_MSG_MAX_SIZE) 96 | { 97 | mpSetAlarm(ALARM_ASSERTION_FAIL, message, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA); 98 | } 99 | else 100 | { 101 | mpSetAlarm(ALARM_ASSERTION_FAIL, "Enable debug LAN port from cfg", SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA); 102 | } 103 | g_nodeConfigSettings.debug_broadcast_enabled = FALSE; 104 | ros_debugPorts.enabledPortCount = 0; 105 | } 106 | } 107 | 108 | void Ros_Debug_BroadcastMsg(char* fmt, ...) 109 | { 110 | char str[MAX_DEBUG_MESSAGE_SIZE]; 111 | va_list va; 112 | 113 | if (g_nodeConfigSettings.debug_broadcast_enabled && ros_debugPorts.enabledPortCount == 0) 114 | { 115 | Ros_Debug_Init(); 116 | } 117 | 118 | //Only exit if the broadcast is not enabled and log_to_stdout is false. The message that is built 119 | //for each of those is the same, so as long as it will either be broadcast or logged to stdout, 120 | //the message should be built 121 | if (!g_nodeConfigSettings.debug_broadcast_enabled && !g_nodeConfigSettings.log_to_stdout) 122 | { 123 | return; 124 | } 125 | 126 | bzero(str, MAX_DEBUG_MESSAGE_SIZE); 127 | 128 | va_start(va, fmt); 129 | vsnprintf(str, MAX_DEBUG_MESSAGE_SIZE, fmt, va); 130 | va_end(va); 131 | 132 | // Timestamp 133 | //The timestamp for the message "Found Micro-Ros PC Agent" will be the epoch time (THU 1970-01-01 00:00:00.000) as the global flags 134 | //are set to indicate that the Micro-Ros PC Agent is connected but the first sync of the host time using the micro-ROS agent is yet to occur 135 | struct tm synced_time; 136 | struct timespec tp; 137 | char timestamp[FORMATTED_TIME_SIZE]; 138 | if (g_Ros_Communication_AgentIsConnected) 139 | { 140 | //get synchronized time from the agent 141 | int64_t nanosecs = rmw_uros_epoch_nanos(); 142 | Ros_Nanos_To_Timespec(nanosecs, &tp); 143 | } 144 | else 145 | { 146 | //rmw_uros_epoch_nanos cannot sync with agent because it's not connected 147 | clock_gettime(CLOCK_REALTIME, &tp); 148 | } 149 | localtime_r(&tp.tv_sec, &synced_time); 150 | strftime(timestamp, FORMATTED_TIME_SIZE, "%Y-%m-%d %H:%M:%S", &synced_time); 151 | snprintf(timestamp + strlen(timestamp), FORMATTED_TIME_SIZE - strlen(timestamp), ".%06d ", (int)tp.tv_nsec / 1000); 152 | 153 | // Pre - pending the timestamp to the debug message 154 | size_t timestamp_length = Ros_strnlen(timestamp, FORMATTED_TIME_SIZE); 155 | size_t debug_message_length = Ros_strnlen(str, MAX_DEBUG_MESSAGE_SIZE); 156 | if (timestamp_length + debug_message_length + 1 < MAX_DEBUG_MESSAGE_SIZE) 157 | { 158 | // Move existing contents of str buffer to the end by Timestamp_Length to make space 159 | //for the timestamp and avoiding overwriting the debug message during the move 160 | memmove(str + timestamp_length, str, debug_message_length); 161 | // Copy the timestamp stored in Formatted_time buffer to the beginning of str buffer 162 | memcpy(str, timestamp, timestamp_length); 163 | } 164 | 165 | 166 | if (g_nodeConfigSettings.debug_broadcast_enabled) 167 | { 168 | for (int i = 0; i < ros_debugPorts.enabledPortCount; i++) 169 | { 170 | mpSendTo(ros_debugPorts.debugSocket[i], str, strlen(str), 0, (struct sockaddr*)&(ros_debugPorts.destAddr[i]), sizeof(struct sockaddr_in)); 171 | } 172 | } 173 | 174 | 175 | if (g_nodeConfigSettings.log_to_stdout) 176 | puts(str); 177 | } 178 | 179 | void Ros_Debug_LogToConsole(char* fmt, ...) 180 | { 181 | char str[MAX_DEBUG_MESSAGE_SIZE]; 182 | va_list va; 183 | 184 | bzero(str, MAX_DEBUG_MESSAGE_SIZE); 185 | 186 | va_start(va, fmt); 187 | vsnprintf(str, MAX_DEBUG_MESSAGE_SIZE, fmt, va); 188 | va_end(va); 189 | 190 | printf(APPLICATION_NAME ": %s\n", str); 191 | } 192 | -------------------------------------------------------------------------------- /src/Debug.h: -------------------------------------------------------------------------------- 1 | //Debug.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_DEBUG_H 9 | #define MOTOROS2_DEBUG_H 10 | #if defined (YRC1000) 11 | #define MAX_NETWORK_PORTS 2 12 | #else 13 | #define MAX_NETWORK_PORTS 1 14 | #endif 15 | 16 | extern void Ros_Debug_SetFromConfig(); 17 | extern void Ros_Debug_BroadcastMsg(char* fmt, ...); 18 | extern void Ros_Debug_LogToConsole(char* fmt, ...); 19 | 20 | #endif // MOTOROS2_DEBUG_H 21 | -------------------------------------------------------------------------------- /src/FauxCommandLineArgs.c: -------------------------------------------------------------------------------- 1 | // FauxCommandLineArgs.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | #include 10 | #include 11 | 12 | 13 | //TODO: Once https://github.com/ros2/rcl/issues/998 is addressed, this would no longer 14 | // be necessary. We could construct rcl_remap_t instances directly without having 15 | // to construct a fake argv and go through rcl_parse_arguments(..). 16 | //NOTE: this is similar to what ComponentManager::create_node_options(..) does. 17 | // 18 | // Assumptions: 19 | // 20 | // 1. 'remap_rules_str' points to a space-separated string of 'from:=to' remap rules 21 | // 2. caller owns 'remap_rules_str' 22 | // 3. 'remap_rules_str' may be clobbered by this function 23 | // 4. 'out_array' is an array of 'char*' of length 'out_array_len' 24 | // 5. all elements of 'out_array' are NULL pointers 25 | // 6. caller owns 'out_array' 26 | // 7. caller will clean up 'out_array' (ie: all memory allocated by this function) 27 | // 8. 'remap_rules_str' does not contain more than MAX_REMAP_RULE_NUM (16) remap rules 28 | // 9. 'remap_rules_str' is no longer than MAX_REMAP_RULE_LEN (255 chars) 29 | // 10. 'out_array_len' represents the nr of slots in 'out_array' 30 | // 31 | // Return value: 32 | // 33 | // < 0: error 34 | // -1: output array too small 35 | // >= 2: number of elements in the fake argv array 36 | // 37 | int Ros_ConstructFauxArgv(char* const remap_rules_str, char* out_array[], size_t out_array_len) 38 | { 39 | const size_t MIN_ARGVS = 1 + 1; // ros args begin and end flags 40 | const size_t MAX_ARGVS = (MAX_REMAP_RULE_NUM * 2) + MIN_ARGVS; 41 | 42 | if (out_array_len < MIN_ARGVS) 43 | return -1; 44 | 45 | size_t num_remap_rules_ = 0; 46 | size_t num_argvs_ = 0; 47 | 48 | // add ROS CLA prefix 49 | out_array[num_argvs_++] = rcutils_strdup(RCL_ROS_ARGS_FLAG, g_motoros2_Allocator); 50 | 51 | rcutils_string_array_t rules = rcutils_get_zero_initialized_string_array(); 52 | rcutils_split(remap_rules_str, ' ', g_motoros2_Allocator, &rules); 53 | for (int i = 0; i < rules.size && num_argvs_ < out_array_len; i += 1) 54 | { 55 | // ros arg indicator 56 | out_array[num_argvs_++] = rcutils_strdup(RCL_SHORT_REMAP_FLAG, g_motoros2_Allocator); 57 | // store ptr to rule 58 | out_array[num_argvs_++] = rcutils_strdup(rules.data[i], g_motoros2_Allocator); 59 | 60 | Ros_Debug_BroadcastMsg("remap rule(%d): %s '%s'", num_remap_rules_, 61 | out_array[num_argvs_-2], out_array[num_argvs_-1]); 62 | 63 | num_remap_rules_++; 64 | 65 | if (num_argvs_ >= MAX_ARGVS) 66 | { 67 | mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Too many remap rules", SUBCODE_CONFIGURATION_TOO_MANY_REMAP_RULES1); 68 | return -1; 69 | } 70 | 71 | if (num_remap_rules_ >= MAX_REMAP_RULE_NUM) 72 | { 73 | mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Too many remap rules", SUBCODE_CONFIGURATION_TOO_MANY_REMAP_RULES2); 74 | return -1; 75 | } 76 | } 77 | 78 | int ret = rcutils_string_array_fini(&rules); 79 | //ignored 80 | RCUTILS_UNUSED(ret); 81 | 82 | if (num_argvs_ == out_array_len) 83 | return -1; 84 | 85 | if ((num_argvs_ - 1) % 2 != 0) 86 | { 87 | mpSetAlarm(ALARM_CONFIGURATION_FAIL, "Invalid remap rule format", SUBCODE_CONFIGURATION_INVALID_REMAP_RULE_FORMAT); 88 | return -1; 89 | } 90 | 91 | // add ROS CLA suffix 92 | out_array[num_argvs_++] = rcutils_strdup( 93 | RCL_ROS_ARGS_EXPLICIT_END_TOKEN, g_motoros2_Allocator); 94 | 95 | Ros_Debug_BroadcastMsg("num parsed remap rules: %d", num_remap_rules_); 96 | Ros_Debug_BroadcastMsg("faux argc: %d", num_argvs_); 97 | 98 | // done 99 | return num_argvs_; 100 | } 101 | 102 | void Ros_CleanupFauxArgv(char** arr, size_t arr_len) 103 | { 104 | for (size_t i = 0; i < arr_len; ++i) 105 | { 106 | if (arr[i]) 107 | g_motoros2_Allocator.deallocate(arr[i], g_motoros2_Allocator.state); 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /src/FauxCommandLineArgs.h: -------------------------------------------------------------------------------- 1 | // FauxCommandLineArgs.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_FAUX_COMMAND_LINE_ARGS_H 9 | #define MOTOROS2_FAUX_COMMAND_LINE_ARGS_H 10 | 11 | extern int Ros_ConstructFauxArgv(char* const remap_rules_str, char* out_array[], size_t out_array_len); 12 | extern void Ros_CleanupFauxArgv(char** arr, size_t arr_len); 13 | 14 | #endif // MOTOROS2_FAUX_COMMAND_LINE_ARGS_H 15 | -------------------------------------------------------------------------------- /src/FileUtilityFunctions.c: -------------------------------------------------------------------------------- 1 | //FileUtilityFunctions.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | BOOL FileUtilityFunctions_ReadLine(int fd, char* buffer, int len) 11 | { 12 | int bytesRead; 13 | char* mid; 14 | 15 | //read more than one line 16 | bzero(buffer, len); 17 | bytesRead = mpRead(fd, buffer, len); 18 | 19 | if (bytesRead == -1 || bytesRead == 0) 20 | return false; 21 | 22 | //find eol 23 | mid = strstr(buffer, "\r\n"); 24 | 25 | //eof 26 | if (mid == NULL) 27 | return true; 28 | 29 | //move file pointer back to eol 30 | int backup = ((bytesRead - (mid - buffer)) * -1) + 2; 31 | mpLseek(fd, backup, SEEK_CUR); 32 | 33 | *mid = '\0'; 34 | 35 | return true; 36 | } 37 | 38 | BOOL FileUtilityFunctions_WriteLine(int fd, const char* fmt, ...) 39 | { 40 | const int MAX_LINE_LEN = 512; 41 | va_list va; 42 | char buffer[MAX_LINE_LEN]; 43 | 44 | va_start(va, fmt); 45 | vsnprintf(buffer, MAX_LINE_LEN, fmt, va); 46 | va_end(va); 47 | 48 | int bytesWrote; 49 | 50 | strcat(buffer, "\r\n"); 51 | 52 | bytesWrote = mpWrite(fd, buffer, Ros_strnlen(buffer, MAX_LINE_LEN)); 53 | 54 | if (bytesWrote == -1 || bytesWrote == 0) 55 | return false; 56 | 57 | return true; 58 | } 59 | -------------------------------------------------------------------------------- /src/FileUtilityFunctions.h: -------------------------------------------------------------------------------- 1 | //FileUtilityFunctions.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_FILE_UTILITY_FUNCTIONS_H 9 | #define MOTOROS2_FILE_UTILITY_FUNCTIONS_H 10 | 11 | extern BOOL FileUtilityFunctions_ReadLine(int fd, char* buffer, int len); 12 | extern BOOL FileUtilityFunctions_WriteLine(int fd, const char* fmt, ...); 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /src/InformCheckerAndGenerator.h: -------------------------------------------------------------------------------- 1 | //InformCheckerAndGenerator.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_INFORM_CHECKER_AND_GENERATOR_H 9 | #define MOTOROS2_INFORM_CHECKER_AND_GENERATOR_H 10 | 11 | // Verify that the INFORM job is properly formatted. If not, 12 | // then build the job automatically 13 | extern void Ros_InformChecker_ValidateJob(); 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /src/MathConstants.h: -------------------------------------------------------------------------------- 1 | //MathConstants.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MATH_CONSTANTS_H 9 | #define MOTOROS2_MATH_CONSTANTS_H 10 | 11 | 12 | // deg->rad (by multiplication) 13 | #define RAD_PER_DEGREE (0.01745329251994) 14 | 15 | // rad->deg (by multiplication) 16 | #define DEGREES_PER_RAD (57.295779513082) 17 | 18 | // macro 19 | #define MICROMETERS_TO_METERS(x) (x * 0.000001) 20 | #define METERS_TO_MICROMETERS(x) (x * 1000000) 21 | #define RAD_TO_DEG_0001(x) (x * DEGREES_PER_RAD * 10000) 22 | #define DEG_0001_TO_RAD(x) ((x / 10000) * RAD_PER_DEGREE) 23 | 24 | // used for comparing floating point values 25 | #define EPSILON_TOLERANCE_DOUBLE (0.001) 26 | 27 | // maximum pulse count deviation for test calculations 28 | #define EPSILON_TOLERANCE_PULSECOUNT (3) 29 | 30 | 31 | #endif // MOTOROS2_MATH_CONSTANTS_H 32 | -------------------------------------------------------------------------------- /src/MemoryAllocation.c: -------------------------------------------------------------------------------- 1 | // MemoryAllocation.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_allocator_t g_motoros2_Allocator; 11 | 12 | void* __motoplus_allocate(size_t size, void* state) 13 | { 14 | RCUTILS_CAN_RETURN_WITH_ERROR_OF(NULL); 15 | 16 | RCUTILS_UNUSED(state); 17 | void* newPtr = mpMalloc(size); 18 | if (!newPtr) 19 | mpSetAlarm(ALARM_ALLOCATION_FAIL, APPLICATION_NAME " OUT OF MEMORY", SUBCODE_ALLOCATION_MALLOC); 20 | return newPtr; 21 | } 22 | 23 | void __motoplus_deallocate(void* pointer, void* state) 24 | { 25 | RCUTILS_UNUSED(state); 26 | mpFree(pointer); 27 | } 28 | 29 | void* __motoplus_reallocate(void* pointer, size_t size, void* state) 30 | { 31 | RCUTILS_CAN_RETURN_WITH_ERROR_OF(NULL); 32 | RCUTILS_UNUSED(state); 33 | 34 | void* newPtr = mpMalloc(size); 35 | if (!newPtr) 36 | { 37 | mpSetAlarm(ALARM_ALLOCATION_FAIL, APPLICATION_NAME " OUT OF MEMORY", SUBCODE_ALLOCATION_REALLOC); 38 | return NULL; 39 | } 40 | 41 | // according to POSIX, pointer == null -> malloc(..) 42 | if (pointer) 43 | { 44 | memcpy(newPtr, pointer, size); 45 | mpFree(pointer); 46 | } 47 | 48 | return newPtr; 49 | } 50 | 51 | void* __motoplus_zero_allocate(size_t number_of_elements, size_t size_of_element, void* state) 52 | { 53 | RCUTILS_CAN_RETURN_WITH_ERROR_OF(NULL); 54 | RCUTILS_UNUSED(state); 55 | 56 | if ((number_of_elements == 0) || (size_of_element == 0)) 57 | { 58 | return NULL; 59 | } 60 | size_t size = size_of_element * number_of_elements; 61 | 62 | void* newPtr = mpMalloc(size); 63 | if (!newPtr) 64 | mpSetAlarm(ALARM_ALLOCATION_FAIL, APPLICATION_NAME " OUT OF MEMORY", SUBCODE_ALLOCATION_CALLOC); 65 | if (newPtr) 66 | bzero(newPtr, size); 67 | 68 | return newPtr; 69 | } 70 | 71 | void Ros_Allocation_Initialize(rcl_allocator_t* const allocator) 72 | { 73 | //initialize memory allocation for microros 74 | allocator->allocate = __motoplus_allocate; 75 | allocator->deallocate = __motoplus_deallocate; 76 | allocator->reallocate = __motoplus_reallocate; 77 | allocator->zero_allocate = __motoplus_zero_allocate; 78 | allocator->state = NULL; 79 | 80 | //configure Micro-ROS to use our allocator. Errors are fatal. 81 | motoRosAssert(rcutils_set_default_allocator(allocator), SUBCODE_FAIL_MEM_ALLOC_CFG); 82 | } 83 | -------------------------------------------------------------------------------- /src/MemoryAllocation.h: -------------------------------------------------------------------------------- 1 | // MemoryAllocation.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MEMORY_ALLOCATION_H 9 | #define MOTOROS2_MEMORY_ALLOCATION_H 10 | 11 | //------------------------------------ 12 | //Dynamic Memory 13 | //------------------------------------ 14 | extern void Ros_Allocation_Initialize(rcl_allocator_t* const allocator); 15 | 16 | extern rcl_allocator_t g_motoros2_Allocator; 17 | 18 | #endif // MOTOROS2_MEMORY_ALLOCATION_H 19 | -------------------------------------------------------------------------------- /src/MemoryTracing.h: -------------------------------------------------------------------------------- 1 | // MemoryTracing.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MEMORY_TRACING_H 9 | #define MOTOROS2_MEMORY_TRACING_H 10 | 11 | 12 | #ifdef MOTOROS2_MEM_TRACE_ENABLE 13 | 14 | 15 | // NOTE: cannot wrap this in a do '{} while(0)' as otherwise the variables 16 | // would be declared in that scope -- and cannot be referenced later 17 | #define MOTOROS2_MEM_TRACE_START(name) \ 18 | uint64_t __mem_trace_point_start_##name = mpNumBytesFree(); 19 | 20 | 21 | #define MOTOROS2_MEM_TRACE_MEM_FREE(name) do \ 22 | { \ 23 | Ros_Debug_BroadcastMsg("%s(..):%s: memory free: %llu bytes", \ 24 | __func__, #name, (uint64_t) mpNumBytesFree()); \ 25 | } while(0); 26 | 27 | 28 | #define MOTOROS2_MEM_TRACE_REPORT(name) do \ 29 | { \ 30 | uint64_t __mem_trace_point_end_##name = mpNumBytesFree(); \ 31 | /* overflow possible, but unlikely given the max mem of YRC1000/DX200 */ \ 32 | int64_t __mem_trace_point_delta_##name = \ 33 | __mem_trace_point_end_##name - __mem_trace_point_start_##name; \ 34 | Ros_Debug_BroadcastMsg("%s(..):%s: memory free: %llu bytes, delta: %+lld", \ 35 | __func__, #name, \ 36 | __mem_trace_point_end_##name, __mem_trace_point_delta_##name); \ 37 | } while(0); 38 | 39 | 40 | #else // MOTOROS2_MEM_TRACE_ENABLE 41 | 42 | 43 | // no-ops if disabled 44 | #define MOTOROS2_MEM_TRACE_START(name) 45 | #define MOTOROS2_MEM_TRACE_MEM_FREE(name) 46 | #define MOTOROS2_MEM_TRACE_REPORT(name) 47 | 48 | 49 | #endif // MOTOROS2_MEM_TRACE_ENABLE 50 | 51 | 52 | #endif // MOTOROS2_MEMORY_TRACING_H 53 | -------------------------------------------------------------------------------- /src/MotionControl.h: -------------------------------------------------------------------------------- 1 | //MotionControl.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MOTION_CONTROL_H 9 | #define MOTOROS2_MOTION_CONTROL_H 10 | 11 | #define START_MAX_PULSE_DEVIATION 30 12 | 13 | #define MOTION_START_TIMEOUT 5000 // in milliseconds 14 | #define MOTION_START_CHECK_PERIOD 50 // in millisecond 15 | #define MOTION_START_ERROR_MESSAGE_LENGTH 256 16 | #define MOTION_STOP_TIMEOUT 20 17 | 18 | typedef enum 19 | { 20 | MOTION_MODE_INACTIVE, 21 | MOTION_MODE_TRAJECTORY, 22 | MOTION_MODE_POINTQUEUE 23 | } MOTION_MODE; 24 | 25 | extern Init_Trajectory_Status Ros_MotionControl_InitTrajectory(control_msgs__action__FollowJointTrajectory_SendGoal_Request* pending_ros_goal_request); 26 | extern void Ros_MotionControl_IncMoveLoopStart(); 27 | extern void Ros_MotionControl_AddToIncQueueProcess(CtrlGroup* ctrlGroup); 28 | extern UINT16 Ros_MotionControl_ProcessQueuedTrajectoryPoint(motoros2_interfaces__srv__QueueTrajPoint_Request* request); 29 | extern BOOL Ros_MotionControl_AddPulseIncPointToQ(CtrlGroup* ctrlGroup, Incremental_data const* dataToEnQ); 30 | extern BOOL Ros_MotionControl_HasDataInQueue(); 31 | extern BOOL Ros_MotionControl_HasDataToProcess(); 32 | extern BOOL Ros_MotionControl_IsRosControllingMotion(); 33 | extern int Ros_MotionControl_GetQueueCnt(int groupNo); 34 | extern BOOL Ros_MotionControl_StopMotion(BOOL bKeepJobRunning); 35 | extern BOOL Ros_MotionControl_ClearQ_All(); 36 | extern MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_runtime_c__String* responseMessage); 37 | extern void Ros_MotionControl_StopTrajMode(); 38 | 39 | extern BOOL Ros_MotionControl_IsMotionMode_Trajectory(); 40 | extern BOOL Ros_MotionControl_IsMotionMode_PointQueue(); 41 | extern BOOL Ros_MotionControl_IsMotionMode_RawStreaming(); 42 | 43 | extern void Ros_MotionControl_ValidateMotionModeIsOk(); 44 | 45 | #endif // MOTOROS2_MOTION_CONTROL_H 46 | -------------------------------------------------------------------------------- /src/MotoROS.h: -------------------------------------------------------------------------------- 1 | //MotoROS.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_MOTOROS_H 9 | #define MOTOROS2_MOTOROS_H 10 | 11 | #define APPLICATION_NAME "MotoROS2" 12 | #define APPLICATION_VERSION "0.2.0" 13 | 14 | #include "motoPlus.h" 15 | 16 | #include "motoplus_libmicroros_config.h" 17 | 18 | //============================================ 19 | // Micro-ROS headers 20 | //============================================ 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | // TODO: remove when Foxy, Galactic and Humble are no longer supported 47 | #if defined(MOTOPLUS_LIBMICROROS_ROS2_IS_FOXY) || defined(MOTOPLUS_LIBMICROROS_ROS2_IS_GALACTIC) || defined(MOTOPLUS_LIBMICROROS_ROS2_IS_HUMBLE) 48 | #include 49 | #else 50 | #include 51 | #endif 52 | 53 | //============================================ 54 | // Data types for communication 55 | //============================================ 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | #include 78 | #include 79 | #include 80 | #include 81 | 82 | //============================================ 83 | // MotoROS 84 | //============================================ 85 | #include "MotoPlusExterns.h" 86 | #include "Debug.h" 87 | #include "FileUtilityFunctions.h" 88 | #include "CommunicationExecutor.h" 89 | #include "Quaternion_Conversion.h" 90 | #include "ErrorHandling.h" 91 | #include "MemoryAllocation.h" 92 | #include "MemoryTracing.h" 93 | #include "CmosParameterExtraction.h" 94 | #include "ActionServer_FJT.h" 95 | #include "CtrlGroup.h" 96 | #include "ControllerStatusIO.h" 97 | #include "PositionMonitor.h" 98 | #include "ServiceQueueTrajPoint.h" 99 | #include "ServiceReadWriteIO.h" 100 | #include "ServiceResetError.h" 101 | #include "ServiceStartTrajMode.h" 102 | #include "ServiceStartPointQueueMode.h" 103 | #include "ServiceStopTrajMode.h" 104 | #include "ServiceSelectMotionTool.h" 105 | #include "MotionControl.h" 106 | #include "ConfigFile.h" 107 | #include "RosApiNameConstants.h" 108 | #include "TimeConversionUtils.h" 109 | #include "Tests_CtrlGroup.h" 110 | #include "Tests_TestUtils.h" 111 | #include "Tests_RosMotoPlusConversionUtils.h" 112 | #include "Tests_ControllerStatusIO.h" 113 | #include "Tests_ActionServer_FJT.h" 114 | #include "Tests_TimeConversionUtils.h" 115 | #include "FauxCommandLineArgs.h" 116 | #include "InformCheckerAndGenerator.h" 117 | #include "MathConstants.h" 118 | #include "MotoROS_PlatformLib.h" 119 | #include "Ros_mpGetRobotCalibrationData.h" 120 | #include "RosMotoPlusConversionUtils.h" 121 | 122 | extern void Ros_Sleep(float milliseconds); 123 | 124 | #endif // MOTOROS2_MOTOROS_H 125 | -------------------------------------------------------------------------------- /src/PositionMonitor.h: -------------------------------------------------------------------------------- 1 | // PositionMonitor.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_POSITION_MONITOR_H 9 | #define MOTOROS2_POSITION_MONITOR_H 10 | 11 | typedef enum 12 | { 13 | tfLink_WorldToBase = 0, 14 | tfLink_BaseToFlange, 15 | tfLink_FlangeToTool0, 16 | tfLink_FlangeToTcp, 17 | 18 | NUMBER_TRANSFORM_LINKS_PER_ROBOT 19 | } TransformLinkIndex; 20 | 21 | typedef struct 22 | { 23 | rcl_publisher_t jointStateAllGroups; 24 | rcl_publisher_t transform; 25 | } PositionMonitor_Publishers; 26 | extern PositionMonitor_Publishers g_publishers_PositionMonitor; 27 | 28 | typedef struct 29 | { 30 | sensor_msgs__msg__JointState* jointStateAllGroups; 31 | tf2_msgs__msg__TFMessage* transform; 32 | } PositionMonitor_Messages; 33 | extern PositionMonitor_Messages g_messages_PositionMonitor; 34 | 35 | extern void Ros_PositionMonitor_Initialize(); 36 | extern void Ros_PositionMonitor_Cleanup(); 37 | 38 | extern void Ros_PositionMonitor_UpdateLocation(); 39 | 40 | #endif // MOTOROS2_POSITION_MONITOR_H 41 | -------------------------------------------------------------------------------- /src/Quaternion_Conversion.c: -------------------------------------------------------------------------------- 1 | // ============================================================================= 2 | // 3 | // QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(..) adapted from: 4 | // 5 | // SPDX-FileCopyrightText: 2011, John Fuller 6 | // SPDX-License-Identifier: BSD-2-Clause 7 | // 8 | // Originally from: 9 | // 10 | // https://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors 11 | // 12 | // Original license: 13 | // 14 | // All rights reserved. 15 | // 16 | // Redistribution and use in source and binary forms, with or without 17 | // modification, are permitted provided that the following conditions are met : 18 | // 19 | // * Redistributions of source code must retain the above copyright notice, this 20 | // list of conditions and the following disclaimer. 21 | // 22 | // * Redistributions in binary form must reproduce the above copyright notice, 23 | // this list of conditionsand the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution 25 | // 26 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | // DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 30 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | // DAMAGES(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | // OR TORT(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 35 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | // 37 | // ============================================================================= 38 | // 39 | // QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(..) adapted from: 40 | // 41 | // SPDX-FileCopyrightText: 2010 - 2020 three.js authors 42 | // SPDX-License-Identifier: MIT 43 | // 44 | // Originally from: 45 | // 46 | // https://github.com/mrdoob/three.js 47 | // 48 | // Original license: 49 | // 50 | // The MIT License 51 | // 52 | // Copyright (c) 2010 - 2020 three.js authors 53 | // 54 | // Permission is hereby granted, free of charge, to any person obtaining a copy 55 | // of this softwareand associated documentation files(the "Software"), to deal 56 | // in the Software without restriction, including without limitation the rights 57 | // to use, copy, modify, merge, publish, distribute, sublicense, and /or sell 58 | // copies of the Software, and to permit persons to whom the Software is 59 | // furnished to do so, subject to the following conditions : 60 | // 61 | // The above copyright noticeand this permission notice shall be included in 62 | // all copies or substantial portions of the Software. 63 | // 64 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 65 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 66 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE 67 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 68 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 69 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 70 | // THE SOFTWARE. 71 | // 72 | // ============================================================================= 73 | 74 | 75 | #include "MotoROS.h" 76 | 77 | #ifndef max 78 | #define max(x, y) (((x) < (y)) ? (y) : (x)) 79 | #endif 80 | 81 | #ifndef min 82 | #define min(x, y) (((x) < (y)) ? (x) : (y)) 83 | #endif 84 | 85 | double QuatConversion_clamp(double value, double _min, double _max) 86 | { 87 | return max(_min, min(_max, value)); 88 | } 89 | 90 | void QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(LONG rx_deg, LONG ry_deg, LONG rz_deg, Quaternion* q) 91 | { 92 | // convert Euler angles to radians, and already assign to the quaternion's 93 | // fields to avoid using (more) temporary storage 94 | // 95 | // NOTE: assumption: Euler angles are in 0.1 milli-degrees (as is the case 96 | // for MP_COORD orientation fields) 97 | q->x = rx_deg * RAD_PER_DEGREE * .0001; 98 | q->y = ry_deg * RAD_PER_DEGREE * .0001; 99 | q->z = rz_deg * RAD_PER_DEGREE * .0001; 100 | 101 | const double c1 = cos(q->x / 2); 102 | const double c2 = cos(q->y / 2); 103 | const double c3 = cos(q->z / 2); 104 | 105 | const double s1 = sin(q->x / 2); 106 | const double s2 = sin(q->y / 2); 107 | const double s3 = sin(q->z / 2); 108 | 109 | q->x = s1 * c2 * c3 - c1 * s2 * s3; 110 | q->y = c1 * s2 * c3 + s1 * c2 * s3; 111 | q->z = c1 * c2 * s3 - s1 * s2 * c3; 112 | q->w = c1 * c2 * c3 + s1 * s2 * s3; 113 | } 114 | 115 | void QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(const Quaternion* const q, LONG* rx_deg, LONG* ry_deg, LONG* rz_deg) 116 | { 117 | const double x = q->x, y = q->y, z = q->z, w = q->w; 118 | const double x2 = x + x, y2 = y + y, z2 = z + z; 119 | const double xx = x * x2, xy = x * y2, xz = x * z2; 120 | const double yy = y * y2, yz = y * z2, zz = z * z2; 121 | const double wx = w * x2, wy = w * y2, wz = w * z2; 122 | 123 | const double sx = 1.0, sy = 1.0, sz = 1.0; 124 | 125 | double elements[16]; 126 | 127 | elements[0] = (1 - (yy + zz)) * sx; 128 | elements[1] = (xy + wz) * sx; 129 | elements[2] = (xz - wy) * sx; 130 | elements[3] = 0; 131 | 132 | elements[4] = (xy - wz) * sy; 133 | elements[5] = (1 - (xx + zz)) * sy; 134 | elements[6] = (yz + wx) * sy; 135 | elements[7] = 0; 136 | 137 | elements[8] = (xz + wy) * sz; 138 | elements[9] = (yz - wx) * sz; 139 | elements[10] = (1 - (xx + yy)) * sz; 140 | elements[11] = 0; 141 | 142 | elements[12] = 0; 143 | elements[13] = 0; 144 | elements[14] = 0; 145 | elements[15] = 1; 146 | 147 | double m11 = elements[0], m12 = elements[4], m13 = elements[8]; 148 | double m21 = elements[1], m22 = elements[5], m23 = elements[9]; 149 | double m31 = elements[2], m32 = elements[6], m33 = elements[10]; 150 | 151 | // these are unused 152 | // TODO(gavanderhoorn): decide whether to remove these instead 153 | (void) m13; 154 | (void) m23; 155 | 156 | // convert radians to Euler anles 157 | // 158 | // NOTE: assumption: Euler angles should be in 0.1 milli-degrees (as those 159 | // are used for MP_COORD orientation fields) 160 | *ry_deg = (LONG)(asin(-QuatConversion_clamp(m31, -1, 1)) * DEGREES_PER_RAD * 10000); 161 | 162 | if (abs(m31) < 0.9999999) 163 | { 164 | *rx_deg = (LONG)(atan2(m32, m33) * DEGREES_PER_RAD * 10000); 165 | *rz_deg = (LONG)(atan2(m21, m11) * DEGREES_PER_RAD * 10000); 166 | } 167 | else 168 | { 169 | *rx_deg = 0; 170 | *rz_deg = (LONG)(atan2(-m12, m22) * DEGREES_PER_RAD * 10000); 171 | } 172 | } 173 | -------------------------------------------------------------------------------- /src/Quaternion_Conversion.h: -------------------------------------------------------------------------------- 1 | //Quaternion_Conversion.h 2 | 3 | // ============================================================================= 4 | // 5 | // QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(..) adapted from: 6 | // 7 | // SPDX-FileCopyrightText: 2011, John Fuller 8 | // SPDX-License-Identifier: BSD-2-Clause 9 | // 10 | // Originally from: 11 | // 12 | // https://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors 13 | // 14 | // Original license: 15 | // 16 | // All rights reserved. 17 | // 18 | // Redistribution and use in source and binary forms, with or without 19 | // modification, are permitted provided that the following conditions are met : 20 | // 21 | // * Redistributions of source code must retain the above copyright notice, this 22 | // list of conditions and the following disclaimer. 23 | // 24 | // * Redistributions in binary form must reproduce the above copyright notice, 25 | // this list of conditionsand the following disclaimer in the documentation 26 | // and/or other materials provided with the distribution 27 | // 28 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 29 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 30 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 31 | // DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 32 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 33 | // DAMAGES(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 34 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 35 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 36 | // OR TORT(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 37 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 38 | // 39 | // ============================================================================= 40 | // 41 | // QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(..) adapted from: 42 | // 43 | // SPDX-FileCopyrightText: 2010 - 2020 three.js authors 44 | // SPDX-License-Identifier: MIT 45 | // 46 | // Originally from: 47 | // 48 | // https://github.com/mrdoob/three.js 49 | // 50 | // Original license: 51 | // 52 | // The MIT License 53 | // 54 | // Copyright (c) 2010 - 2020 three.js authors 55 | // 56 | // Permission is hereby granted, free of charge, to any person obtaining a copy 57 | // of this softwareand associated documentation files(the "Software"), to deal 58 | // in the Software without restriction, including without limitation the rights 59 | // to use, copy, modify, merge, publish, distribute, sublicense, and /or sell 60 | // copies of the Software, and to permit persons to whom the Software is 61 | // furnished to do so, subject to the following conditions : 62 | // 63 | // The above copyright noticeand this permission notice shall be included in 64 | // all copies or substantial portions of the Software. 65 | // 66 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 67 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 68 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE 69 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 70 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 71 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 72 | // THE SOFTWARE. 73 | // 74 | // ============================================================================= 75 | 76 | 77 | #ifndef MOTOROS2_QUATERNION_CONVERSION_H 78 | #define MOTOROS2_QUATERNION_CONVERSION_H 79 | 80 | typedef geometry_msgs__msg__Quaternion Quaternion; 81 | 82 | /** 83 | * Convert Euler angles in degrees (ZYX-intrinsic) to their quaternion representation. 84 | * 85 | * NOTE: Euler angles are expected to be in one-tenth milli-degrees (ie: 0.0001 deg), 86 | * such as are used for MP_COORD orientation fields. 87 | */ 88 | extern void QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(LONG rx_deg, LONG ry_deg, LONG rz_deg, Quaternion* q); 89 | 90 | /** 91 | * Convert the quaternion q to its Euler angle (ZYX-intrinsic) representation. 92 | * 93 | * NOTE: Euler angles will be in one-tenth milli-degrees (ie: 0.0001 deg), such 94 | * as are used for MP_COORD orientation fields. 95 | */ 96 | extern void QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(const Quaternion* const q, LONG* rx_deg, LONG* ry_deg, LONG* rz_deg); 97 | 98 | #endif // MOTOROS2_QUATERNION_CONVERSION_H 99 | -------------------------------------------------------------------------------- /src/RosApiNameConstants.h: -------------------------------------------------------------------------------- 1 | //RosApiNameConstants.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_ROS_API_NAME_CONSTANTS_H 9 | #define MOTOROS2_ROS_API_NAME_CONSTANTS_H 10 | 11 | 12 | //============================================ 13 | // Topic, service and action server names 14 | //============================================ 15 | #define TOPIC_NAME_TF "tf" 16 | #define TOPIC_NAME_ROBOT_STATUS "robot_status" 17 | #define TOPIC_NAME_JOINT_STATES "joint_states" 18 | 19 | #define SERVICE_NAME_READ_SINGLE_IO "read_single_io" 20 | #define SERVICE_NAME_READ_GROUP_IO "read_group_io" 21 | #define SERVICE_NAME_WRITE_SINGLE_IO "write_single_io" 22 | #define SERVICE_NAME_WRITE_GROUP_IO "write_group_io" 23 | #define SERVICE_NAME_READ_MREGISTER "read_mregister" 24 | #define SERVICE_NAME_WRITE_MREGISTER "write_mregister" 25 | #define SERVICE_NAME_RESET_ERROR "reset_error" 26 | #define SERVICE_NAME_START_TRAJ_MODE "start_traj_mode" 27 | #define SERVICE_NAME_START_POINT_QUEUE_MODE "start_point_queue_mode" 28 | #define SERVICE_NAME_STOP_TRAJ_MODE "stop_traj_mode" 29 | #define SERVICE_NAME_QUEUE_TRAJ_POINT "queue_traj_point" 30 | #define SERVICE_NAME_SELECT_MOTION_TOOL "select_motion_tool" 31 | 32 | #define ACTION_NAME_FOLLOW_JOINT_TRAJECTORY "follow_joint_trajectory" 33 | 34 | 35 | #endif // MOTOROS2_ROS_API_NAME_CONSTANTS_H 36 | -------------------------------------------------------------------------------- /src/RosMotoPlusConversionUtils.c: -------------------------------------------------------------------------------- 1 | //RosMotoPlusConversionUtils.c 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | 11 | void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose) 12 | { 13 | ros_pose->position.x = MICROMETERS_TO_METERS(mp_coord->x); 14 | ros_pose->position.y = MICROMETERS_TO_METERS(mp_coord->y); 15 | ros_pose->position.z = MICROMETERS_TO_METERS(mp_coord->z); 16 | QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion( 17 | mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_pose->orientation); 18 | } 19 | 20 | void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform) 21 | { 22 | ros_transform->translation.x = MICROMETERS_TO_METERS(mp_coord->x); 23 | ros_transform->translation.y = MICROMETERS_TO_METERS(mp_coord->y); 24 | ros_transform->translation.z = MICROMETERS_TO_METERS(mp_coord->z); 25 | QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion( 26 | mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_transform->rotation); 27 | } 28 | -------------------------------------------------------------------------------- /src/RosMotoPlusConversionUtils.h: -------------------------------------------------------------------------------- 1 | //RosMotoPlusConversionUtils.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H 9 | #define MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H 10 | 11 | 12 | /** 13 | * Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Pose`. 14 | * 15 | * Unit conversion is also performed by this function: 16 | * 17 | * - `MP_COORD` positions are converted from micro-meters to meters 18 | * - `MP_COORD` orientations are converted from milli-degrees to radians (and 19 | * subsequently to a `Quaternion`) 20 | * 21 | * @param mp_coord Input M+ `MP_COORD` instance 22 | * @param ros_pose Output `geometry_msgs/msg/Pose` 23 | */ 24 | extern void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose); 25 | 26 | 27 | /** 28 | * Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Transform`. 29 | * 30 | * Unit conversion is also performed by this function: 31 | * 32 | * - `MP_COORD` positions are converted from micro-meters to meters 33 | * - `MP_COORD` orientations are converted from milli-degrees to radians (and 34 | * subsequently to a `Quaternion`) 35 | * 36 | * @param mp_coord Input M+ `MP_COORD` instance 37 | * @param ros_transform Output `geometry_msgs/msg/Pose` 38 | */ 39 | extern void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform); 40 | 41 | 42 | #endif // MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H 43 | -------------------------------------------------------------------------------- /src/Ros_mpGetRobotCalibrationData.h: -------------------------------------------------------------------------------- 1 | // Ros_mpGetRobotCalibrationData.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H 9 | #define MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H 10 | 11 | 12 | #include "motoPlus.h" 13 | 14 | 15 | #if defined (FS100) || defined (DX200) 16 | 17 | // from mpSysCtrl.h 18 | 19 | #ifdef MP_RC_XYZ_NUM 20 | #error "DX200/FS100 M+ SDK conflicts with MP_RC_XYZ_NUM definition" 21 | #endif 22 | 23 | #ifdef MP_RC_CALIB_P_NUM 24 | #error "DX200/FS100 M+ SDK conflicts with MP_RC_CALIB_P_NUM definition" 25 | #endif 26 | 27 | #ifdef MP_RC_RB_P_NUM 28 | #error "DX200/FS100 M+ SDK conflicts with MP_RC_RB_P_NUM definition" 29 | #endif 30 | 31 | #define MP_RC_XYZ_NUM (3) 32 | #define MP_RC_CALIB_P_NUM (5) 33 | #define MP_RC_RB_P_NUM (2) 34 | 35 | typedef struct { 36 | CHAR tool_no; 37 | UCHAR reserved[3]; 38 | LONG axis_data[MP_GRP_AXES_NUM]; 39 | } MP_RB_CALIB_POS; 40 | 41 | typedef struct { 42 | ULONG grp_no; 43 | MP_RB_CALIB_POS calib_pos[MP_RC_CALIB_P_NUM][MP_RC_RB_P_NUM]; 44 | } MP_RB_CALIB_INFO; 45 | 46 | typedef struct { 47 | MP_RB_CALIB_INFO m_rb; 48 | MP_RB_CALIB_INFO s_rb; 49 | LONG pos_uow[MP_RC_XYZ_NUM]; 50 | LONG ang_uow[MP_RC_XYZ_NUM]; 51 | } MP_RB_CALIB_DATA; 52 | 53 | #endif 54 | 55 | extern void Ros_mpGetRobotCalibrationData_Initialize(); 56 | extern void Ros_mpGetRobotCalibrationData_Cleanup(); 57 | extern LONG Ros_mpGetRobotCalibrationData(ULONG file_no, MP_RB_CALIB_DATA *rData); 58 | 59 | 60 | #endif // MOTOROS2_ROS_MP_GET_ROBOT_CALIBRATION_DATA_H 61 | -------------------------------------------------------------------------------- /src/ServiceQueueTrajPoint.c: -------------------------------------------------------------------------------- 1 | //ServiceQueueTrajPoint.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_service_t g_serviceQueueTrajPoint; 11 | 12 | typedef motoros2_interfaces__srv__QueueTrajPoint_Request QueueTrajPointRequest; 13 | typedef motoros2_interfaces__srv__QueueTrajPoint_Response QueueTrajPointResponse; 14 | 15 | ServiceQueueTrajPoint_Messages g_messages_QueueTrajPoint; 16 | 17 | void Ros_ServiceQueueTrajPoint_Initialize() 18 | { 19 | MOTOROS2_MEM_TRACE_START(svc_queue_point_init); 20 | 21 | //-------------- 22 | const rosidl_service_type_support_t* type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, QueueTrajPoint); 23 | 24 | rcl_ret_t ret; 25 | ret = rclc_service_init_default(&g_serviceQueueTrajPoint, &g_microRosNodeInfo.node, type_support, SERVICE_NAME_QUEUE_TRAJ_POINT); 26 | motoRos_RCLAssertOK(ret, SUBCODE_FAIL_CREATE_SERVICE_QUEUE_POINT); 27 | 28 | //-------------- 29 | //I'm intentionally allocating for all possible axes, rather than the actual number used. 30 | //If the user submits a point with too many axes, the message would get lost in the ether 31 | //if there's not enough memory to deserialize it. So, I would be unable to see the message 32 | //and provide an appropriate error code. 33 | int maxAxes = MAX_CONTROLLABLE_GROUPS * MP_GRP_AXES_NUM; 34 | 35 | //TODO: Update to use micro_ros_utilities for allocation 36 | g_messages_QueueTrajPoint.request = motoros2_interfaces__srv__QueueTrajPoint_Request__create(); 37 | rosidl_runtime_c__String__Sequence__init(&g_messages_QueueTrajPoint.request->joint_names, maxAxes); 38 | 39 | for (int i = 0; i < maxAxes; i += 1) 40 | rosidl_runtime_c__String__assign(&g_messages_QueueTrajPoint.request->joint_names.data[i], "012345678901234567890123456789012"); 41 | 42 | rosidl_runtime_c__float64__Sequence__init(&g_messages_QueueTrajPoint.request->point.positions, maxAxes); 43 | rosidl_runtime_c__float64__Sequence__init(&g_messages_QueueTrajPoint.request->point.velocities, maxAxes); 44 | rosidl_runtime_c__float64__Sequence__init(&g_messages_QueueTrajPoint.request->point.accelerations, maxAxes); 45 | rosidl_runtime_c__float64__Sequence__init(&g_messages_QueueTrajPoint.request->point.effort, maxAxes); 46 | 47 | //-------------- 48 | g_messages_QueueTrajPoint.response = motoros2_interfaces__srv__QueueTrajPoint_Response__create(); 49 | 50 | //-------------- 51 | MOTOROS2_MEM_TRACE_REPORT(svc_queue_point_init); 52 | } 53 | 54 | void Ros_ServiceQueueTrajPoint_Cleanup() 55 | { 56 | rcl_ret_t ret; 57 | MOTOROS2_MEM_TRACE_START(svc_queue_point_fini); 58 | 59 | Ros_Debug_BroadcastMsg("Cleanup service queue point"); 60 | ret = rcl_service_fini(&g_serviceQueueTrajPoint, &g_microRosNodeInfo.node); 61 | if (ret != RCL_RET_OK) 62 | Ros_Debug_BroadcastMsg("Failed cleaning up " SERVICE_NAME_QUEUE_TRAJ_POINT " service: %d", ret); 63 | 64 | motoros2_interfaces__srv__QueueTrajPoint_Request__destroy(g_messages_QueueTrajPoint.request); 65 | 66 | motoros2_interfaces__srv__QueueTrajPoint_Response__destroy(g_messages_QueueTrajPoint.response); 67 | 68 | MOTOROS2_MEM_TRACE_REPORT(svc_queue_point_fini); 69 | } 70 | 71 | void Ros_ServiceQueueTrajPoint_Trigger(const void* request_msg, void* response_msg) 72 | { 73 | QueueTrajPointRequest* request = (QueueTrajPointRequest*)request_msg; 74 | QueueTrajPointResponse* response = (QueueTrajPointResponse*)response_msg; 75 | 76 | Ros_Debug_BroadcastMsg("Streaming point received"); 77 | 78 | if (!Ros_MotionControl_IsMotionMode_PointQueue()) 79 | { 80 | response->result_code.value = motoros2_interfaces__msg__QueueResultEnum__WRONG_MODE; 81 | rosidl_runtime_c__String__assign(&response->message, 82 | motoros2_interfaces__msg__QueueResultEnum__WRONG_MODE_STR); 83 | 84 | Ros_Debug_BroadcastMsg("Point rejected due to wrong trajectory-mode"); 85 | } 86 | else 87 | { 88 | const char* initMsg; 89 | response->result_code.value = Ros_MotionControl_ProcessQueuedTrajectoryPoint(request); 90 | 91 | switch (response->result_code.value) 92 | { 93 | case motoros2_interfaces__msg__QueueResultEnum__SUCCESS: 94 | rosidl_runtime_c__String__assign(&response->message, 95 | motoros2_interfaces__msg__QueueResultEnum__SUCCESS_STR); 96 | break; 97 | 98 | case motoros2_interfaces__msg__QueueResultEnum__BUSY: 99 | rosidl_runtime_c__String__assign(&response->message, 100 | motoros2_interfaces__msg__QueueResultEnum__BUSY_STR); 101 | break; 102 | 103 | case motoros2_interfaces__msg__QueueResultEnum__INVALID_JOINT_LIST: 104 | rosidl_runtime_c__String__assign(&response->message, 105 | motoros2_interfaces__msg__QueueResultEnum__INVALID_JOINT_LIST_STR); 106 | break; 107 | 108 | default: 109 | //Otherwise it should be a forwarded Init_Trajectory_Status value that was returned by Ros_MotionControl_ProcessQueuedTrajectoryPoint() 110 | initMsg = Ros_ErrorHandling_Init_Trajectory_Status_ToString((Init_Trajectory_Status)response->result_code.value); 111 | //If there is an Init_Trajectory_Status string associated with that forwarded value, assign that string for the result message. Otherwise assign a generic failure string. 112 | if (strcmp(initMsg, motoros2_interfaces__msg__InitTrajEnum__INIT_TRAJ_UNSPECIFIED_STR) == 0) { 113 | rosidl_runtime_c__String__assign(&response->message, 114 | "Unspecified error occurred while adding the trajectory point to the queue."); 115 | } 116 | else 117 | { 118 | rosidl_runtime_c__String__assign(&response->message, 119 | initMsg); 120 | } 121 | break; 122 | } 123 | Ros_Debug_BroadcastMsg("Reply (%d): %s", response->result_code.value, response->message.data); 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /src/ServiceQueueTrajPoint.h: -------------------------------------------------------------------------------- 1 | //ServiceQueueTrajPoint.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_QUEUE_TRAJ_POINT_H 9 | #define MOTOROS2_SERVICE_QUEUE_TRAJ_POINT_H 10 | 11 | extern rcl_service_t g_serviceQueueTrajPoint; 12 | 13 | typedef struct 14 | { 15 | motoros2_interfaces__srv__QueueTrajPoint_Request* request; 16 | motoros2_interfaces__srv__QueueTrajPoint_Response* response; 17 | } ServiceQueueTrajPoint_Messages; 18 | extern ServiceQueueTrajPoint_Messages g_messages_QueueTrajPoint; 19 | 20 | extern void Ros_ServiceQueueTrajPoint_Initialize(); 21 | extern void Ros_ServiceQueueTrajPoint_Cleanup(); 22 | 23 | extern void Ros_ServiceQueueTrajPoint_Trigger(const void* request_msg, void* response_msg); 24 | 25 | #endif // MOTOROS2_SERVICE_QUEUE_TRAJ_POINT_H 26 | -------------------------------------------------------------------------------- /src/ServiceReadWriteIO.h: -------------------------------------------------------------------------------- 1 | //ServiceReadWriteIO.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_READ_WRITE_IO_H 9 | #define MOTOROS2_SERVICE_READ_WRITE_IO_H 10 | 11 | extern rcl_service_t g_serviceReadSingleIO; 12 | extern rcl_service_t g_serviceReadGroupIO; 13 | extern rcl_service_t g_serviceWriteSingleIO; 14 | extern rcl_service_t g_serviceWriteGroupIO; 15 | extern rcl_service_t g_serviceReadMRegister; 16 | extern rcl_service_t g_serviceWriteMRegister; 17 | 18 | typedef struct 19 | { 20 | motoros2_interfaces__srv__ReadSingleIO_Request req_single_io_read; 21 | motoros2_interfaces__srv__ReadSingleIO_Response resp_single_io_read; 22 | 23 | motoros2_interfaces__srv__ReadGroupIO_Request req_group_io_read; 24 | motoros2_interfaces__srv__ReadGroupIO_Response resp_group_io_read; 25 | 26 | motoros2_interfaces__srv__WriteSingleIO_Request req_single_io_write; 27 | motoros2_interfaces__srv__WriteSingleIO_Response resp_single_io_write; 28 | 29 | motoros2_interfaces__srv__WriteGroupIO_Request req_group_io_write; 30 | motoros2_interfaces__srv__WriteGroupIO_Response resp_group_io_write; 31 | 32 | motoros2_interfaces__srv__ReadMRegister_Request req_mreg_read; 33 | motoros2_interfaces__srv__ReadMRegister_Response resp_mreg_read; 34 | 35 | motoros2_interfaces__srv__WriteMRegister_Request req_mreg_write; 36 | motoros2_interfaces__srv__WriteMRegister_Response resp_mreg_write; 37 | } ServiceReadWriteIO_Messages; 38 | 39 | extern ServiceReadWriteIO_Messages g_messages_ReadWriteIO; 40 | 41 | void Ros_ServiceReadWriteIO_Initialize(); 42 | void Ros_ServiceReadWriteIO_Cleanup(); 43 | 44 | void Ros_ServiceReadSingleIO_Trigger(const void* request_msg, void* response_msg); 45 | void Ros_ServiceReadGroupIO_Trigger(const void* request_msg, void* response_msg); 46 | void Ros_ServiceWriteSingleIO_Trigger(const void* request_msg, void* response_msg); 47 | void Ros_ServiceWriteGroupIO_Trigger(const void* request_msg, void* response_msg); 48 | void Ros_ServiceReadMRegister_Trigger(const void* request_msg, void* response_msg); 49 | void Ros_ServiceWriteMRegister_Trigger(const void* request_msg, void* response_msg); 50 | 51 | 52 | #endif // MOTOROS2_SERVICE_READ_WRITE_IO_H 53 | -------------------------------------------------------------------------------- /src/ServiceResetError.c: -------------------------------------------------------------------------------- 1 | //ServiceResetError.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | #define RESET_ERROR_CHECK_TIMEOUT 5000 // ms 11 | #define RESET_ERROR_CHECK_PERIOD 50 // ms 12 | 13 | rcl_service_t g_serviceResetError; 14 | 15 | ServiceResetError_Messages g_messages_ResetError; 16 | 17 | typedef motoros2_interfaces__srv__ResetError_Response ResetErrorResponse; 18 | 19 | 20 | void Ros_ServiceResetError_Initialize() 21 | { 22 | MOTOROS2_MEM_TRACE_START(svc_reset_error_init); 23 | 24 | const rosidl_service_type_support_t* type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, ResetError); 25 | 26 | rcl_ret_t ret = rclc_service_init_default(&g_serviceResetError, &g_microRosNodeInfo.node, type_support, SERVICE_NAME_RESET_ERROR); 27 | motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_RESET_ERROR, "Failed to init service (%d)", (int)ret); 28 | 29 | rosidl_runtime_c__String__init(&g_messages_ResetError.response.message); 30 | 31 | MOTOROS2_MEM_TRACE_REPORT(svc_reset_error_init); 32 | } 33 | 34 | void Ros_ServiceResetError_Cleanup() 35 | { 36 | rcl_ret_t ret; 37 | MOTOROS2_MEM_TRACE_START(svc_reset_error_fini); 38 | 39 | Ros_Debug_BroadcastMsg("Cleanup service reset"); 40 | ret = rcl_service_fini(&g_serviceResetError, &g_microRosNodeInfo.node); 41 | if (ret != RCL_RET_OK) 42 | Ros_Debug_BroadcastMsg("Failed cleaning up reset service: %d", ret); 43 | rosidl_runtime_c__String__fini(&g_messages_ResetError.response.message); 44 | 45 | MOTOROS2_MEM_TRACE_REPORT(svc_reset_error_fini); 46 | } 47 | 48 | void Ros_ServiceResetError_Trigger(const void* request_msg, void* response_msg) 49 | { 50 | int ret; 51 | MP_STD_RSP_DATA rData; 52 | 53 | RCL_UNUSED(request_msg); 54 | ResetErrorResponse* response = (ResetErrorResponse*)response_msg; 55 | 56 | Ros_Debug_BroadcastMsg("reset: attempting to reset controller"); 57 | 58 | // we do not allow resetting the controller unless in remote mode 59 | if (!Ros_Controller_IsRemote()) 60 | { 61 | rosidl_runtime_c__String__assign(&response->message, "Pendant is not in REMOTE mode"); 62 | response->result_code.value = MOTION_NOT_READY_NOT_REMOTE; 63 | goto DONE; 64 | } 65 | 66 | // not 'real' errors/alarms, but internal error flags 67 | Ros_Controller_Reset_PflDuringRosMove(); 68 | Ros_Controller_Reset_MpIncMoveError(); 69 | 70 | // Check for condition that can be fixed remotely 71 | if (Ros_Controller_IsError()) 72 | { 73 | // Cancel error 74 | bzero(&rData, sizeof(rData)); 75 | ret = mpCancelError(&rData); 76 | 77 | // if mpCancelError failed, we can't recover 78 | if (ret != 0) 79 | { 80 | rosidl_runtime_c__String__assign(&response->message, "Robot has an active ERROR"); 81 | response->result_code.value = MOTION_NOT_READY_ERROR; 82 | goto DONE; 83 | } 84 | } 85 | 86 | // Major alarms cannot be reset remotely 87 | if (Ros_Controller_IsMajorAlarm()) 88 | { 89 | rosidl_runtime_c__String__assign(&response->message, "Major alarm active. Cannot be reset. Check teach pendant"); 90 | response->result_code.value = MOTION_NOT_READY_MAJOR_ALARM; 91 | goto DONE; 92 | } 93 | 94 | // Other alarm types can be reset remotely 95 | if (Ros_Controller_IsAlarm()) 96 | { 97 | // Reset alarm 98 | bzero(&rData, sizeof(rData)); 99 | ret = mpResetAlarm(&rData); 100 | 101 | // if mpResetAlarm failed, we can't recover 102 | if (ret != 0) 103 | { 104 | rosidl_runtime_c__String__assign(&response->message, "Robot has an active ALARM"); 105 | response->result_code.value = MOTION_NOT_READY_ALARM; 106 | goto DONE; 107 | } 108 | 109 | // wait for the Alarm reset confirmation 110 | int checkCount; 111 | for (checkCount = 0; checkCount < RESET_ERROR_CHECK_TIMEOUT; checkCount += RESET_ERROR_CHECK_PERIOD) 112 | { 113 | // Update status 114 | Ros_Controller_IoStatusUpdate(); 115 | 116 | if (Ros_Controller_IsAlarm() == FALSE) 117 | break; 118 | 119 | Ros_Sleep(RESET_ERROR_CHECK_PERIOD); 120 | } 121 | if (Ros_Controller_IsAlarm()) 122 | { 123 | rosidl_runtime_c__String__assign(&response->message, "Robot has an active ALARM"); 124 | response->result_code.value = MOTION_NOT_READY_ALARM; 125 | goto DONE; 126 | } 127 | } 128 | 129 | // at this point alarms/errors should be cleared, so report to caller 130 | rosidl_runtime_c__String__assign(&response->message, "success"); 131 | response->result_code.value = MOTION_READY; 132 | 133 | DONE: 134 | Ros_Debug_BroadcastMsg("reset: %s", response->message.data); 135 | } 136 | -------------------------------------------------------------------------------- /src/ServiceResetError.h: -------------------------------------------------------------------------------- 1 | //ServiceResetError.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_RESET_ERROR_H 9 | #define MOTOROS2_SERVICE_RESET_ERROR_H 10 | 11 | extern rcl_service_t g_serviceResetError; 12 | 13 | typedef struct 14 | { 15 | motoros2_interfaces__srv__ResetError_Request request; 16 | motoros2_interfaces__srv__ResetError_Response response; 17 | } ServiceResetError_Messages; 18 | extern ServiceResetError_Messages g_messages_ResetError; 19 | 20 | extern void Ros_ServiceResetError_Initialize(); 21 | extern void Ros_ServiceResetError_Cleanup(); 22 | 23 | extern void Ros_ServiceResetError_Trigger(const void* request_msg, void* response_msg); 24 | 25 | #endif // MOTOROS2_SERVICE_RESET_ERROR_H 26 | -------------------------------------------------------------------------------- /src/ServiceSelectMotionTool.c: -------------------------------------------------------------------------------- 1 | //ServiceSelectMotionTool.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_service_t g_serviceSelectMotionTool; 11 | 12 | ServiceSelectMotionTool_Messages g_messages_SelectMotionTool; 13 | 14 | typedef motoros2_interfaces__srv__SelectMotionTool_Response SelectMotionToolResponse; 15 | typedef motoros2_interfaces__srv__SelectMotionTool_Request SelectMotionToolRequest; 16 | 17 | 18 | void Ros_ServiceSelectMotionTool_Initialize() 19 | { 20 | MOTOROS2_MEM_TRACE_START(svc_select_tool_init); 21 | 22 | const rosidl_service_type_support_t* type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, SelectMotionTool); 23 | 24 | rcl_ret_t ret = rclc_service_init_default(&g_serviceSelectMotionTool, &g_microRosNodeInfo.node, type_support, SERVICE_NAME_SELECT_MOTION_TOOL); 25 | motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_SELECT_MOTION_TOOL, "Failed to init service (%d)", (int)ret); 26 | 27 | rosidl_runtime_c__String__init(&g_messages_SelectMotionTool.response.message); 28 | 29 | MOTOROS2_MEM_TRACE_REPORT(svc_select_tool_init); 30 | } 31 | 32 | void Ros_ServiceSelectMotionTool_Cleanup() 33 | { 34 | rcl_ret_t ret; 35 | MOTOROS2_MEM_TRACE_START(svc_select_tool_fini); 36 | 37 | Ros_Debug_BroadcastMsg("Cleanup service select tool"); 38 | ret = rcl_service_fini(&g_serviceSelectMotionTool, &g_microRosNodeInfo.node); 39 | if (ret != RCL_RET_OK) 40 | Ros_Debug_BroadcastMsg("Failed cleaning up select tool service: %d", ret); 41 | rosidl_runtime_c__String__fini(&g_messages_SelectMotionTool.response.message); 42 | 43 | MOTOROS2_MEM_TRACE_REPORT(svc_select_tool_fini); 44 | } 45 | 46 | void Ros_ServiceSelectMotionTool_Trigger(const void* request_msg, void* response_msg) 47 | { 48 | Ros_Debug_BroadcastMsg("%s: entry", __func__); 49 | 50 | //NOTE: this implementation is different from the MotoROS1 implementation 51 | // as it doesn't call mpSetToolNo(..). 52 | // Rationale: mpSetToolNo(..) changes the tool used for teaching 53 | // INFORM positions using the pendant. As there is no support in 54 | // MotoROS2 for teaching INFORM positions, there is no need to 55 | // change the 'teach tool'. 56 | // Being able to change the tool used by the incremental-motion 57 | // interface MotoROS2 uses (ie: mpExRcsIncrementMove et al.) *is* 58 | // necessary, hence the implementation here in this service. 59 | 60 | SelectMotionToolResponse* response = (SelectMotionToolResponse*)response_msg; 61 | SelectMotionToolRequest* request = (SelectMotionToolRequest*)request_msg; 62 | 63 | //init response message 64 | response->success = false; 65 | 66 | Ros_Debug_BroadcastMsg("%s: requested: grp no: %u, tool: %u", 67 | __func__, request->group_number, request->tool_number); 68 | 69 | // we do not allow changing the tool unless in remote mode 70 | if (!Ros_Controller_IsRemote()) 71 | { 72 | rosidl_runtime_c__String__assign(&response->message, motoros2_interfaces__msg__SelectionResultCodes__INVALID_CONTROLLER_STATE_STR); 73 | response->result_code.value = motoros2_interfaces__msg__SelectionResultCodes__INVALID_CONTROLLER_STATE; 74 | goto DONE; 75 | } 76 | 77 | //make sure a valid group is specified (group_number is unsigned, so cannot be < 0) 78 | if (request->group_number >= g_Ros_Controller.numGroup) 79 | { 80 | rosidl_runtime_c__String__assign(&response->message, motoros2_interfaces__msg__SelectionResultCodes__INVALID_CONTROL_GROUP_STR); 81 | response->result_code.value = motoros2_interfaces__msg__SelectionResultCodes__INVALID_CONTROL_GROUP; 82 | goto DONE; 83 | } 84 | 85 | //make sure a valid tool is specified (tool_number is unsigned, so cannot be < 0) 86 | if (request->tool_number > MAX_VALID_TOOL_INDEX) 87 | { 88 | rosidl_runtime_c__String__assign(&response->message, motoros2_interfaces__msg__SelectionResultCodes__INVALID_SELECTION_INDEX_STR); 89 | response->result_code.value = motoros2_interfaces__msg__SelectionResultCodes__INVALID_SELECTION_INDEX; 90 | goto DONE; 91 | } 92 | 93 | //set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..)) 94 | //NOTE: this will change the 'motion tool' ONLY for those increments which 95 | // haven't yet been added to the increment queue. See also the ROS 2 96 | // 'select_tool' service definition file in motoros2_interfaces. 97 | g_Ros_Controller.ctrlGroups[request->group_number]->tool = request->tool_number; 98 | 99 | //report to caller 100 | rosidl_runtime_c__String__assign(&response->message, motoros2_interfaces__msg__SelectionResultCodes__OK_STR); 101 | response->success = true; 102 | response->result_code.value = motoros2_interfaces__msg__SelectionResultCodes__OK; 103 | 104 | DONE: 105 | Ros_Debug_BroadcastMsg("%s: exit: '%s' (%lu)", __func__, response->message.data, 106 | (unsigned long) response->result_code.value); 107 | } 108 | -------------------------------------------------------------------------------- /src/ServiceSelectMotionTool.h: -------------------------------------------------------------------------------- 1 | //ServiceSelectMotionTool.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_SELECT_TOOL_H 9 | #define MOTOROS2_SERVICE_SELECT_TOOL_H 10 | 11 | extern rcl_service_t g_serviceSelectMotionTool; 12 | 13 | typedef struct 14 | { 15 | motoros2_interfaces__srv__SelectMotionTool_Request request; 16 | motoros2_interfaces__srv__SelectMotionTool_Response response; 17 | } ServiceSelectMotionTool_Messages; 18 | extern ServiceSelectMotionTool_Messages g_messages_SelectMotionTool; 19 | 20 | extern void Ros_ServiceSelectMotionTool_Initialize(); 21 | extern void Ros_ServiceSelectMotionTool_Cleanup(); 22 | 23 | extern void Ros_ServiceSelectMotionTool_Trigger(const void* request_msg, void* response_msg); 24 | 25 | #endif // MOTOROS2_SERVICE_SELECT_TOOL_H 26 | -------------------------------------------------------------------------------- /src/ServiceStartPointQueueMode.c: -------------------------------------------------------------------------------- 1 | //ServiceStartPointQueueMode.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_service_t g_serviceStartPointQueueMode; 11 | 12 | ServicePointQueueMode_Messages g_messages_StartPointQueueMode; 13 | 14 | // shorten the typename a little, locally 15 | typedef motoros2_interfaces__srv__StartPointQueueMode_Response StartPointQueueMode_Response; 16 | 17 | void Ros_ServiceStartPointQueueMode_Initialize() 18 | { 19 | MOTOROS2_MEM_TRACE_START(svc_start_point_queue_mode_init); 20 | 21 | rcl_ret_t ret = rclc_service_init_default(&g_serviceStartPointQueueMode, &g_microRosNodeInfo.node, 22 | ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, StartPointQueueMode), 23 | SERVICE_NAME_START_POINT_QUEUE_MODE); 24 | motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_START_QUEUE_MODE, "Failed to init service (%d)", (int)ret); 25 | 26 | rosidl_runtime_c__String__init(&g_messages_StartPointQueueMode.response.message); 27 | 28 | MOTOROS2_MEM_TRACE_REPORT(svc_start_point_queue_mode_init); 29 | } 30 | 31 | void Ros_ServiceStartPointQueueMode_Cleanup() 32 | { 33 | MOTOROS2_MEM_TRACE_START(svc_start_point_queue_mode_fini); 34 | 35 | rcl_ret_t ret; 36 | 37 | Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_START_POINT_QUEUE_MODE); 38 | ret = rcl_service_fini(&g_serviceStartPointQueueMode, &g_microRosNodeInfo.node); 39 | if (ret != RCL_RET_OK) 40 | Ros_Debug_BroadcastMsg( 41 | "Failed cleaning up " SERVICE_NAME_START_POINT_QUEUE_MODE " service: %d", ret); 42 | rosidl_runtime_c__String__fini(&g_messages_StartPointQueueMode.response.message); 43 | 44 | MOTOROS2_MEM_TRACE_REPORT(svc_start_point_queue_mode_fini); 45 | } 46 | 47 | void Ros_ServiceStartPointQueueMode_Trigger(const void* request_msg, void* response_msg) 48 | { 49 | RCL_UNUSED(request_msg); 50 | StartPointQueueMode_Response* response = (StartPointQueueMode_Response*) response_msg; 51 | 52 | // trust .. 53 | response->result_code.value = MOTION_READY; 54 | rosidl_runtime_c__String__assign(&response->message, ""); 55 | 56 | MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_POINTQUEUE, &response->message); 57 | if (motion_result_code != MOTION_READY) 58 | { 59 | // update response 60 | response->result_code.value = motion_result_code; 61 | 62 | //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function 63 | if (motion_result_code != MOTION_NOT_READY_ERROR) { 64 | // map to human readable string 65 | rosidl_runtime_c__String__assign(&response->message, 66 | Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); 67 | } 68 | 69 | Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, 70 | response->message.data, response->result_code.value); 71 | } 72 | else 73 | { 74 | Ros_Debug_BroadcastMsg("%s: activated", __func__); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/ServiceStartPointQueueMode.h: -------------------------------------------------------------------------------- 1 | //ServiceStartPointQueueMode.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_START_POINT_QUEUE_MODE_H 9 | #define MOTOROS2_SERVICE_START_POINT_QUEUE_MODE_H 10 | 11 | 12 | extern rcl_service_t g_serviceStartPointQueueMode; 13 | 14 | typedef struct 15 | { 16 | motoros2_interfaces__srv__StartPointQueueMode_Request request; 17 | motoros2_interfaces__srv__StartPointQueueMode_Response response; 18 | } ServicePointQueueMode_Messages; 19 | extern ServicePointQueueMode_Messages g_messages_StartPointQueueMode; 20 | 21 | extern void Ros_ServiceStartPointQueueMode_Initialize(); 22 | extern void Ros_ServiceStartPointQueueMode_Cleanup(); 23 | 24 | extern void Ros_ServiceStartPointQueueMode_Trigger(const void* request_msg, void* response_msg); 25 | 26 | 27 | #endif // MOTOROS2_SERVICE_START_POINT_QUEUE_MODE_H 28 | -------------------------------------------------------------------------------- /src/ServiceStartTrajMode.c: -------------------------------------------------------------------------------- 1 | //ServiceStartTrajMode.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_service_t g_serviceStartTrajMode; 11 | 12 | ServiceStartTrajMode_Messages g_messages_StartTrajMode; 13 | 14 | // shorten the typename a little, locally 15 | typedef motoros2_interfaces__srv__StartTrajMode_Response StartTrajMode_Response; 16 | 17 | void Ros_ServiceStartTrajMode_Initialize() 18 | { 19 | MOTOROS2_MEM_TRACE_START(svc_start_traj_mode_init); 20 | 21 | rcl_ret_t ret = rclc_service_init_default(&g_serviceStartTrajMode, &g_microRosNodeInfo.node, 22 | ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, StartTrajMode), 23 | SERVICE_NAME_START_TRAJ_MODE); 24 | motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_START_TRAJ_MODE, "Failed to init service (%d)", (int)ret); 25 | 26 | rosidl_runtime_c__String__init(&g_messages_StartTrajMode.response.message); 27 | 28 | MOTOROS2_MEM_TRACE_REPORT(svc_start_traj_mode_init); 29 | } 30 | 31 | void Ros_ServiceStartTrajMode_Cleanup() 32 | { 33 | MOTOROS2_MEM_TRACE_START(svc_start_traj_mode_fini); 34 | 35 | rcl_ret_t ret; 36 | 37 | Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_START_TRAJ_MODE); 38 | ret = rcl_service_fini(&g_serviceStartTrajMode, &g_microRosNodeInfo.node); 39 | if (ret != RCL_RET_OK) 40 | Ros_Debug_BroadcastMsg( 41 | "Failed cleaning up " SERVICE_NAME_START_TRAJ_MODE " service: %d", ret); 42 | rosidl_runtime_c__String__fini(&g_messages_StartTrajMode.response.message); 43 | 44 | MOTOROS2_MEM_TRACE_REPORT(svc_start_traj_mode_fini); 45 | } 46 | 47 | void Ros_ServiceStartTrajMode_Trigger(const void* request_msg, void* response_msg) 48 | { 49 | RCL_UNUSED(request_msg); 50 | StartTrajMode_Response* response = (StartTrajMode_Response*) response_msg; 51 | 52 | // trust .. 53 | response->result_code.value = MOTION_READY; 54 | rosidl_runtime_c__String__assign(&response->message, ""); 55 | 56 | MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_TRAJECTORY, &response->message); 57 | if (motion_result_code != MOTION_READY) 58 | { 59 | // update response 60 | response->result_code.value = motion_result_code; 61 | 62 | //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function 63 | if (motion_result_code != MOTION_NOT_READY_ERROR) { 64 | // map to human readable string 65 | rosidl_runtime_c__String__assign(&response->message, 66 | Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); 67 | } 68 | 69 | Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, 70 | response->message.data, response->result_code.value); 71 | } 72 | else 73 | { 74 | Ros_Debug_BroadcastMsg("%s: activated", __func__); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/ServiceStartTrajMode.h: -------------------------------------------------------------------------------- 1 | //ServiceStartTrajMode.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_START_TRAJ_MODE_H 9 | #define MOTOROS2_SERVICE_START_TRAJ_MODE_H 10 | 11 | 12 | extern rcl_service_t g_serviceStartTrajMode; 13 | 14 | typedef struct 15 | { 16 | motoros2_interfaces__srv__StartTrajMode_Request request; 17 | motoros2_interfaces__srv__StartTrajMode_Response response; 18 | } ServiceStartTrajMode_Messages; 19 | extern ServiceStartTrajMode_Messages g_messages_StartTrajMode; 20 | 21 | extern void Ros_ServiceStartTrajMode_Initialize(); 22 | extern void Ros_ServiceStartTrajMode_Cleanup(); 23 | 24 | extern void Ros_ServiceStartTrajMode_Trigger(const void* request_msg, void* response_msg); 25 | 26 | 27 | #endif // MOTOROS2_SERVICE_START_TRAJ_MODE_H 28 | -------------------------------------------------------------------------------- /src/ServiceStopTrajMode.c: -------------------------------------------------------------------------------- 1 | //ServiceStopTrajMode.c 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #include "MotoROS.h" 9 | 10 | rcl_service_t g_serviceStopTrajMode; 11 | 12 | ServiceStopTrajMode_Messages g_messages_StopTrajMode; 13 | 14 | void Ros_ServiceStopTrajMode_Initialize() 15 | { 16 | MOTOROS2_MEM_TRACE_START(svc_stop_traj_mode_init); 17 | 18 | const rosidl_service_type_support_t* type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger); 19 | 20 | rcl_ret_t ret = rclc_service_init_default(&g_serviceStopTrajMode, &g_microRosNodeInfo.node, type_support, SERVICE_NAME_STOP_TRAJ_MODE); 21 | motoRosAssert_withMsg(ret == RCL_RET_OK, SUBCODE_FAIL_INIT_SERVICE_STOP_TRAJ_MODE, "Failed to init service (%d)", (int)ret); 22 | 23 | rosidl_runtime_c__String__init(&g_messages_StopTrajMode.response.message); 24 | 25 | MOTOROS2_MEM_TRACE_REPORT(svc_stop_traj_mode_init); 26 | } 27 | 28 | void Ros_ServiceStopTrajMode_Cleanup() 29 | { 30 | MOTOROS2_MEM_TRACE_START(svc_stop_traj_mode); 31 | 32 | rcl_ret_t ret; 33 | 34 | Ros_Debug_BroadcastMsg("Cleanup service stop_traj_mode"); 35 | ret = rcl_service_fini(&g_serviceStopTrajMode, &g_microRosNodeInfo.node); 36 | if (ret != RCL_RET_OK) 37 | Ros_Debug_BroadcastMsg("Failed cleaning up stop_traj_mode service: %d", ret); 38 | rosidl_runtime_c__String__fini(&g_messages_StopTrajMode.response.message); 39 | 40 | MOTOROS2_MEM_TRACE_REPORT(svc_stop_traj_mode); 41 | } 42 | 43 | void Ros_ServiceStopTrajMode_Trigger(const void* request_msg, void* response_msg) 44 | { 45 | Ros_Debug_BroadcastMsg("stop_traj_mode: attempting to stop trajectory mode"); 46 | 47 | std_srvs__srv__Trigger_Response* response = (std_srvs__srv__Trigger_Response*)response_msg; 48 | 49 | // Don't change mode if queue is not empty 50 | if(Ros_MotionControl_HasDataInQueue()) 51 | { 52 | Ros_Debug_BroadcastMsg("stop_traj_mode: increment queue not empty"); 53 | rosidl_runtime_c__String__assign(&response->message, "Can't disable trajectory mode: increment queue not empty"); 54 | response->success = FALSE; 55 | return; 56 | } 57 | 58 | // Stop motion 59 | if(!Ros_MotionControl_StopMotion(/*bKeepJobRunning = */ FALSE)) 60 | { 61 | Ros_Debug_BroadcastMsg("stop_traj_mode: StopMotion failed (unknown error)"); 62 | rosidl_runtime_c__String__assign(&response->message, "Couldn't stop trajectory mode: unknown error"); 63 | response->success = FALSE; 64 | return; 65 | } 66 | 67 | Ros_MotionControl_StopTrajMode(); 68 | 69 | // Set I/O signal 70 | Ros_Controller_SetIOState(IO_FEEDBACK_MP_INCMOVE_DONE, TRUE); 71 | 72 | Ros_Debug_BroadcastMsg("stop_traj_mode: stopped trajectory mode"); 73 | rosidl_runtime_c__String__assign(&response->message, ""); 74 | response->success = TRUE; 75 | } 76 | -------------------------------------------------------------------------------- /src/ServiceStopTrajMode.h: -------------------------------------------------------------------------------- 1 | //ServiceStopTrajMode.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_SERVICE_STOP_TRAJ_MODE_H 9 | #define MOTOROS2_SERVICE_STOP_TRAJ_MODE_H 10 | 11 | extern rcl_service_t g_serviceStopTrajMode; 12 | 13 | typedef struct 14 | { 15 | std_srvs__srv__Trigger_Request request; 16 | std_srvs__srv__Trigger_Response response; 17 | } ServiceStopTrajMode_Messages; 18 | extern ServiceStopTrajMode_Messages g_messages_StopTrajMode; 19 | 20 | extern void Ros_ServiceStopTrajMode_Initialize(); 21 | extern void Ros_ServiceStopTrajMode_Cleanup(); 22 | 23 | extern void Ros_ServiceStopTrajMode_Trigger(const void* request_msg, void* response_msg); 24 | 25 | #endif // MOTOROS2_SERVICE_STOP_TRAJ_MODE_H 26 | -------------------------------------------------------------------------------- /src/Tests_ActionServer_FJT.h: -------------------------------------------------------------------------------- 1 | // Tests_ActionServer_FJT.h 2 | 3 | // SPDX-FileCopyrightText: 2024, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2024, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_TESTS_ACTION_SERVER_FJT_H 9 | #define MOTOROS2_TESTS_TESTS_ACTION_SERVER_FJT_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_ActionServer_FJT(); 14 | 15 | #endif //MOTOROS2_TESTING_ENABLE 16 | 17 | #endif // MOTOROS2_TESTS_TESTS_ACTION_SERVER_FJT_H 18 | -------------------------------------------------------------------------------- /src/Tests_ControllerStatusIO.h: -------------------------------------------------------------------------------- 1 | // Tests_ControllerStatusIO.h 2 | 3 | // SPDX-FileCopyrightText: 2024, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2024, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H 9 | #define MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_ControllerStatusIO(); 14 | 15 | #endif //MOTOROS2_TESTING_ENABLE 16 | 17 | #endif // MOTOROS2_TESTS_CONTROLLER_STATUS_IO_H 18 | -------------------------------------------------------------------------------- /src/Tests_CtrlGroup.h: -------------------------------------------------------------------------------- 1 | // Tests_CtrlGroup.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_CTRL_GROUP_H 9 | #define MOTOROS2_TESTS_CTRL_GROUP_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_CtrlGroup(); 14 | 15 | #endif //MOTOROS2_TESTING_ENABLE 16 | 17 | #endif // MOTOROS2_TESTS_CTRL_GROUP_H 18 | -------------------------------------------------------------------------------- /src/Tests_RosMotoPlusConversionUtils.c: -------------------------------------------------------------------------------- 1 | // Tests_RosMotoPlusConversionUtils.c 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifdef MOTOROS2_TESTING_ENABLE 9 | 10 | #include "MotoROS.h" 11 | 12 | 13 | BOOL Ros_Testing_Ros_MpCoord_To_GeomMsgsPose() 14 | { 15 | BOOL bSuccess = TRUE; 16 | MP_COORD mp_coord_ = { 0 }; 17 | geometry_msgs__msg__Pose ros_pose_ = { { 0 } }; 18 | 19 | mp_coord_.x = 1e6; 20 | mp_coord_.y = 2e6; 21 | mp_coord_.z = 3e6; 22 | mp_coord_.rx = 4; 23 | mp_coord_.ry = 5; 24 | mp_coord_.rz = 6; 25 | 26 | Ros_MpCoord_To_GeomMsgsPose(&mp_coord_, &ros_pose_); 27 | 28 | // position is in micro-meters originally, so the ROS type should have it 29 | // in meters 30 | bSuccess &= Ros_Testing_CompareDouble(1.0, ros_pose_.position.x); 31 | bSuccess &= Ros_Testing_CompareDouble(2.0, ros_pose_.position.y); 32 | bSuccess &= Ros_Testing_CompareDouble(3.0, ros_pose_.position.z); 33 | 34 | // orientation is a Quaternion (and 'in' radians) 35 | // use the inverse (quaternion -> MP_COORD) and compare to the original 36 | LONG rx_ = 0, ry_ = 0, rz_ = 0; 37 | QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient( 38 | &ros_pose_.orientation, &rx_, &ry_, &rz_); 39 | 40 | // NOTE: this is not a strict equals test right now (uses non-zero epsilon) 41 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.rx, rx_); 42 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.ry, ry_); 43 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.rz, rz_); 44 | 45 | Ros_Debug_BroadcastMsg("Testing Ros_MpCoord_To_GeomMsgsPose: %s", bSuccess ? "PASS" : "FAIL"); 46 | return bSuccess; 47 | } 48 | 49 | BOOL Ros_Testing_Ros_MpCoord_To_GeomMsgsTransform() 50 | { 51 | BOOL bSuccess = TRUE; 52 | MP_COORD mp_coord_ = { 0 }; 53 | geometry_msgs__msg__Transform ros_tf_ = { { 0 } }; 54 | 55 | mp_coord_.x = 4e6; 56 | mp_coord_.y = 5e6; 57 | mp_coord_.z = 6e6; 58 | mp_coord_.rx = 1; 59 | mp_coord_.ry = 2; 60 | mp_coord_.rz = 3; 61 | 62 | Ros_MpCoord_To_GeomMsgsTransform(&mp_coord_, &ros_tf_); 63 | 64 | // translation is in micro-meters originally, so the ROS type should have it 65 | // in meters 66 | bSuccess &= Ros_Testing_CompareDouble(4.0, ros_tf_.translation.x); 67 | bSuccess &= Ros_Testing_CompareDouble(5.0, ros_tf_.translation.y); 68 | bSuccess &= Ros_Testing_CompareDouble(6.0, ros_tf_.translation.z); 69 | 70 | // rotation is a Quaternion (and 'in' radians) 71 | // use the inverse (quaternion -> MP_COORD) and compare to the original 72 | LONG rx_ = 0, ry_ = 0, rz_ = 0; 73 | QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient( 74 | &ros_tf_.rotation, &rx_, &ry_, &rz_); 75 | 76 | // NOTE: this is not a strict equals test right now (uses non-zero epsilon) 77 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.rx, rx_); 78 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.ry, ry_); 79 | bSuccess &= Ros_Testing_CompareLong(mp_coord_.rz, rz_); 80 | 81 | Ros_Debug_BroadcastMsg("Testing Ros_MpCoord_To_GeomMsgsTransform: %s", bSuccess ? "PASS" : "FAIL"); 82 | return bSuccess; 83 | } 84 | 85 | BOOL Ros_Testing_RosMotoPlusConversionUtils() 86 | { 87 | BOOL bSuccess = TRUE; 88 | 89 | bSuccess &= Ros_Testing_Ros_MpCoord_To_GeomMsgsPose(); 90 | bSuccess &= Ros_Testing_Ros_MpCoord_To_GeomMsgsTransform(); 91 | 92 | return bSuccess; 93 | } 94 | 95 | #endif //MOTOROS2_TESTING_ENABLE 96 | -------------------------------------------------------------------------------- /src/Tests_RosMotoPlusConversionUtils.h: -------------------------------------------------------------------------------- 1 | // Tests_RosMotoPlusConversionUtils.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H 9 | #define MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_RosMotoPlusConversionUtils(); 14 | 15 | #endif //MOTOROS2_TESTING_ENABLE 16 | 17 | #endif // MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H 18 | -------------------------------------------------------------------------------- /src/Tests_TestUtils.c: -------------------------------------------------------------------------------- 1 | // Tests_TestUtils.c 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifdef MOTOROS2_TESTING_ENABLE 9 | 10 | #include "MotoROS.h" 11 | 12 | BOOL Ros_Testing_CompareDouble(double a, double b) 13 | { 14 | BOOL ok = fabs(a - b) < EPSILON_TOLERANCE_DOUBLE; //this approach has potential flaws, but it works just fine for these simple tests 15 | 16 | if (!ok) 17 | Ros_Debug_BroadcastMsg("Fail: %.04f != %.04f", a, b); 18 | 19 | return ok; 20 | } 21 | 22 | // This is necessary because rounding errors can cause the converted positions to be wrong by a pulse-count or two 23 | BOOL Ros_Testing_CompareLong(long a, long b) 24 | { 25 | BOOL ok = fabs(a - b) < EPSILON_TOLERANCE_PULSECOUNT; //this approach has potential flaws, but it works just fine for these simple tests 26 | 27 | if (!ok) 28 | Ros_Debug_BroadcastMsg("Fail: %d != %d", a, b); 29 | 30 | return ok; 31 | } 32 | 33 | BOOL Ros_Testing_INT64_Equals(INT64 a, INT64 b) 34 | { 35 | BOOL ok = a == b; 36 | 37 | if (!ok) 38 | Ros_Debug_BroadcastMsg("Fail: %lld != %lld", a, b); 39 | 40 | return ok; 41 | } 42 | 43 | BOOL Ros_Testing_Timespec_Equals(const struct timespec* lhs, const struct timespec* rhs) 44 | { 45 | BOOL ok = lhs->tv_sec == rhs->tv_sec; 46 | 47 | if (!ok) 48 | { 49 | Ros_Debug_BroadcastMsg("Fail timespec seconds: %d != %d", lhs->tv_sec, rhs->tv_sec); 50 | } 51 | 52 | ok &= lhs->tv_nsec == rhs->tv_nsec; 53 | 54 | if (!ok) 55 | { 56 | Ros_Debug_BroadcastMsg("Fail timespec nanos: %d != %d", lhs->tv_nsec, rhs->tv_nsec); 57 | } 58 | return ok; 59 | } 60 | 61 | #endif //MOTOROS2_TESTING_ENABLE 62 | -------------------------------------------------------------------------------- /src/Tests_TestUtils.h: -------------------------------------------------------------------------------- 1 | // Tests_TestUtils.h 2 | 3 | // SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_TEST_UTILS_H 9 | #define MOTOROS2_TESTS_TEST_UTILS_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_CompareDouble(double a, double b); 14 | extern BOOL Ros_Testing_CompareLong(long a, long b); 15 | extern BOOL Ros_Testing_INT64_Equals(INT64 a, INT64 b); 16 | extern BOOL Ros_Testing_Timespec_Equals(const struct timespec* lhs, const struct timespec* rhs); 17 | #endif //MOTOROS2_TESTING_ENABLE 18 | 19 | #endif // MOTOROS2_TESTS_TEST_UTILS_H 20 | -------------------------------------------------------------------------------- /src/Tests_TimeConversionUtils.h: -------------------------------------------------------------------------------- 1 | // Tests_TimeConversionUtils.h 2 | 3 | // SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2025, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TESTS_TIME_CONVERSION_UTILS_H 9 | #define MOTOROS2_TESTS_TIME_CONVERSION_UTILS_H 10 | 11 | #ifdef MOTOROS2_TESTING_ENABLE 12 | 13 | extern BOOL Ros_Testing_TimeConversionUtils(); 14 | 15 | #endif //MOTOROS2_TESTING_ENABLE 16 | 17 | #endif // MOTOROS2_TESTS_TIME_CONVERSION_UTILS_H 18 | -------------------------------------------------------------------------------- /src/TimeConversionUtils.h: -------------------------------------------------------------------------------- 1 | //TimeConversionUtils.h 2 | 3 | // SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | // SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | // 6 | // SPDX-License-Identifier: Apache-2.0 7 | 8 | #ifndef MOTOROS2_TIME_CONVERSION_UTILS_H 9 | #define MOTOROS2_TIME_CONVERSION_UTILS_H 10 | 11 | 12 | #include "MotoROS.h" 13 | 14 | static inline void floor_divide(INT64 dividend, INT64 divisor, INT64* quotient, INT64* remainder) 15 | { 16 | *quotient = dividend / divisor; 17 | // If the division has a remainder, and the result is negative, 18 | // we need to adjust to floor the result 19 | if (dividend % divisor != 0 && ((dividend < 0) != (divisor < 0))) 20 | { 21 | *quotient -= 1; 22 | } 23 | 24 | *remainder = abs(dividend - *quotient * divisor); 25 | } 26 | 27 | static inline INT64 Ros_Duration_Msg_To_Millis(builtin_interfaces__msg__Duration const* const x) 28 | { 29 | return (x->sec * (INT64)1e3) + (x->nanosec / (INT64)1e6); 30 | } 31 | 32 | static inline INT64 Ros_Duration_Msg_To_Nanos(builtin_interfaces__msg__Duration const* const x) 33 | { 34 | return (x->sec * (INT64)1e9) + (x->nanosec); 35 | } 36 | 37 | static inline void Ros_Millis_To_Duration_Msg(INT64 x, builtin_interfaces__msg__Duration* const y) 38 | { 39 | INT64 remainder, quotient; 40 | floor_divide(x, (INT64)1e3, "ient, &remainder); 41 | y->sec = quotient; 42 | y->nanosec = remainder * (INT64)1e6; 43 | } 44 | 45 | static inline void Ros_Nanos_To_Duration_Msg(INT64 x, builtin_interfaces__msg__Duration* const y) 46 | { 47 | INT64 remainder, quotient; 48 | floor_divide(x, (INT64)1e9, "ient, &remainder); 49 | y->sec = quotient; 50 | y->nanosec = remainder; 51 | } 52 | 53 | static inline INT64 Ros_Time_Msg_To_Millis(builtin_interfaces__msg__Time const* const x) 54 | { 55 | return (x->sec * (INT64)1e3) + (x->nanosec / ((INT64)1e6)); 56 | } 57 | 58 | static inline INT64 Ros_Time_Msg_To_Nanos(builtin_interfaces__msg__Time const* const x) 59 | { 60 | return (x->sec * (INT64)1e9) + x->nanosec; 61 | } 62 | 63 | static inline void Ros_Millis_To_Time_Msg(INT64 x, builtin_interfaces__msg__Time* const y) 64 | { 65 | INT64 remainder, quotient; 66 | floor_divide(x, (INT64)1e3, "ient, &remainder); 67 | y->sec = quotient; 68 | y->nanosec = remainder * (INT64)1e6; 69 | } 70 | 71 | static inline void Ros_Nanos_To_Time_Msg(INT64 x, builtin_interfaces__msg__Time* const y) 72 | { 73 | INT64 remainder, quotient; 74 | floor_divide(x, (INT64)1e9, "ient, &remainder); 75 | y->sec = quotient; 76 | y->nanosec = remainder; 77 | } 78 | 79 | static inline void Ros_Nanos_To_Timespec(INT64 x, struct timespec* const y) 80 | { 81 | INT64 remainder, quotient; 82 | floor_divide(x, (INT64)1e9, "ient, &remainder); 83 | y->tv_sec = quotient; 84 | y->tv_nsec = remainder; 85 | } 86 | 87 | 88 | #endif // MOTOROS2_TIME_CONVERSION_UTILS_H 89 | -------------------------------------------------------------------------------- /src/YRC1000_foxyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_foxyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_galacticCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_galacticLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_humbleCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_humbleLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_jazzyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000 -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000_jazzyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_foxyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000u -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_foxyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_galacticCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000u -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_galacticLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_humbleCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000u -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_humbleLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_jazzyCompilerArguments.mps: -------------------------------------------------------------------------------- 1 | -DYRC1000u -DMOTOROS2_MEM_TRACE_ENABLE -std=gnu99 -march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -D_WRS_KERNEL -I "~ProjectDir~" ~AdditionalIncludeDir~ -isystem "~IncludeDir~" -O0 ~ObjFileInfo~ -c "~FilePath~" 2 | -------------------------------------------------------------------------------- /src/YRC1000u_jazzyLinkerArguments.mps: -------------------------------------------------------------------------------- 1 | -std=gnu99 -nostdlib -r -Wl,--discard-locals -Wl,--entry=mpUsrRoot ~FileList~ -o "~OutputPath~" 2 | -------------------------------------------------------------------------------- /src/controllers.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | yrcLib_jazzy;yrcLib 9 | yrcmLib_jazzy;yrcmLib 10 | dnLib_jazzy;dnLib 11 | yrcLib_humble;yrcLib 12 | yrcmLib_humble;yrcmLib 13 | dnLib_humble;dnLib 14 | yrcLib_foxy;yrcLib 15 | yrcmLib_foxy;yrcmLib 16 | dnLib_foxy;dnLib 17 | yrcLib_galactic;yrcLib 18 | yrcmLib_galactic;yrcmLib 19 | dnLib_galactic;dnLib 20 | 21 | 22 | 23 | MOTOPLUS-YRC1000 24 | MOTOPLUS-YRC1000micro 25 | MOTOPLUS-DX200 26 | MOTOPLUS-YRC1000 27 | MOTOPLUS-YRC1000micro 28 | MOTOPLUS-DX200 29 | MOTOPLUS-YRC1000 30 | MOTOPLUS-YRC1000micro 31 | MOTOPLUS-DX200 32 | MOTOPLUS-YRC1000 33 | MOTOPLUS-YRC1000micro 34 | MOTOPLUS-DX200 35 | 36 | 37 | 38 | YRC1000\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 39 | YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 40 | DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 41 | YRC1000\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 42 | YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 43 | DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 44 | YRC1000\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 45 | YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 46 | DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 47 | YRC1000\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 48 | YRC1000u\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 49 | DX200\gnu\4.3.3-vxworks-6.9\x86-win32\bin\ccpentium.exe 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/image_comments/flange_vs_tool0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/src/image_comments/flange_vs_tool0.png -------------------------------------------------------------------------------- /src/image_comments/tf_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yaskawa-Global/motoros2/48dd4a1758fec1840d076b1914611670e8e45040/src/image_comments/tf_diagram.png -------------------------------------------------------------------------------- /tools/.gitignore: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 2 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 3 | # 4 | # SPDX-License-Identifier: CC0-1.0 5 | 6 | *_log.txt 7 | -------------------------------------------------------------------------------- /tools/debug_listener.cmd: -------------------------------------------------------------------------------- 1 | @echo off 2 | :: debug_listener.cmd 3 | 4 | :: SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 5 | :: SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 6 | :: 7 | :: SPDX-License-Identifier: Apache-2.0 8 | cd %~dp0 9 | python %~dp0\debug_listener.py 10 | -------------------------------------------------------------------------------- /tools/debug_listener.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. 4 | # SPDX-FileCopyrightText: 2022-2023, Delft University of Technology 5 | # 6 | # SPDX-License-Identifier: Apache-2.0 7 | 8 | import argparse 9 | import asyncio 10 | import datetime 11 | import os 12 | import platform 13 | import socket 14 | import time 15 | from functools import partial 16 | from typing import Tuple, Union, Text 17 | 18 | Address = Tuple[str, int] 19 | 20 | # length of the stamp in a log msg: 'YYYY-MM-DD HH:MM:SS.nnnnnn' 21 | # (where 'nnnnnn' is the microsecond part) 22 | STAMP_STR_LEN=26 23 | 24 | def main(): 25 | default_bcast_port=21789 26 | default_local_bind_addr='0.0.0.0' 27 | 28 | parser = argparse.ArgumentParser( 29 | description='Receives MotoROS2 debug messages and prints them to ' 30 | 'the console and logs them to a file (unless configured not to).', 31 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 32 | parser.add_argument('-p', '--port', dest='listen_port', 33 | default=default_bcast_port, 34 | help='UDP port to listen on') 35 | parser.add_argument('-b', '--bind', dest='listen_ip', 36 | default=default_local_bind_addr, 37 | help='IP address to listen on') 38 | parser.add_argument('-n', '--no-file-sync', action='store_true', 39 | help='Do not log to files, only to console') 40 | args = parser.parse_args() 41 | 42 | # loop and prot 43 | loop = asyncio.get_event_loop() 44 | prot = DebugBroadcastProtocol(loop=loop) 45 | 46 | # always register console sink 47 | prot.register_sink_cb(cb=console_sink_cb) 48 | print('Listening for MotoROS2 debug msgs (format: "[time_msg_rcvd] [ip:port] msg") ..') 49 | print('---') 50 | 51 | # if not configured otherwise, register file cb 52 | if not args.no_file_sync: 53 | stamp = time.strftime('%Y%m%dT%H%M%S') 54 | fname = f"{stamp}_motoros2_debug_log.txt" 55 | # leaks f, but will be closed at program exit 56 | f = open(fname, 'w') 57 | prot.register_sink_cb(cb=partial(file_sink, f=f)) 58 | 59 | try: 60 | # run it 61 | loop.run_until_complete( 62 | loop.create_datagram_endpoint( 63 | lambda: prot, 64 | local_addr=(args.listen_ip, args.listen_port), 65 | reuse_port=True if platform.system().lower() == 'linux' else False, 66 | # this is also needed for receiving broadcasts 67 | allow_broadcast=True)) 68 | loop.run_forever() 69 | except KeyboardInterrupt: 70 | pass 71 | 72 | # convenience: if zero bytes logged, delete output file 73 | if not args.no_file_sync and f.tell() == 0: 74 | print("\nNo messages received, deleting empty log file") 75 | f.close() 76 | os.remove(path=fname) 77 | 78 | 79 | def file_sink(f, msg, stamp, source_addr): 80 | ip, port = source_addr 81 | print(f'[{stamp}] [{ip}:{port}]: {msg}', file=f) 82 | f.flush() 83 | 84 | 85 | def console_sink_cb(msg, stamp, source_addr): 86 | ip, port = source_addr 87 | print(f'[{stamp}] [{ip}:{port}]: {msg}') 88 | 89 | 90 | class DebugBroadcastProtocol(asyncio.DatagramProtocol): 91 | def __init__(self, *, loop: asyncio.AbstractEventLoop = None): 92 | self.loop = asyncio.get_event_loop() if loop is None else loop 93 | self._sink_cbs = [] 94 | 95 | def register_sink_cb(self, cb): 96 | self._sink_cbs.append(cb) 97 | 98 | def connection_made(self, transport: asyncio.transports.DatagramTransport): 99 | self.transport = transport 100 | 101 | def datagram_received(self, data: Union[bytes, Text], addr: Address): 102 | # note: we assume all sinks appreciate strings instead of raw data 103 | msg = data.decode('ascii') 104 | sent_stamp = msg[:STAMP_STR_LEN] 105 | msg = msg[STAMP_STR_LEN+1:] 106 | source_addr = (addr[0], addr[1]) 107 | self._write_to_sinks(msg=msg, stamp=sent_stamp, source_addr=source_addr) 108 | 109 | def _write_to_sinks(self, msg, stamp, source_addr): 110 | for cb in self._sink_cbs: 111 | cb(msg=msg, stamp=stamp, source_addr=source_addr) 112 | 113 | 114 | if __name__ == '__main__': 115 | main() 116 | --------------------------------------------------------------------------------