diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..ab8d07757 --- /dev/null +++ b/.clang-format @@ -0,0 +1,14 @@ +--- +Language: Cpp +BasedOnStyle: Google + +ColumnLimit: 100 +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BreakBeforeBraces: Allman +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +IncludeBlocks: Preserve diff --git a/.github/workflows/README.md b/.github/workflows/README.md new file mode 100644 index 000000000..a9ba3edf8 --- /dev/null +++ b/.github/workflows/README.md @@ -0,0 +1,24 @@ +## Build status + +ROS2 Distro | Branch | Build status | Documentation | Released packages +:---------: | :----: | :----------: | :-----------: | :---------------: +**Rolling** | [`rolling`](https://github.com/StoglRobotics-forks/kuka_experimental/tree/rolling) | [![Rolling Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-main.yml?branch=rolling)
[![Rolling Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-testing.yml?branch=rolling)
[![Rolling Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-main.yml?branch=rolling)
[![Rolling Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-testing.yml?branch=rolling)
[![Rolling Source Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-source-build.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-source-build.yml?branch=rolling) | [![Doxygen Doc Deployment](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml/badge.svg)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml)
[Generated Doc](https://StoglRobotics-forks.github.io/kuka_experimental_Documentation/rolling/html/index.html) | [kuka_experimental](https://index.ros.org/p/kuka_experimental/#rolling) +**Foxy** | [`foxy`](https://github.com/StoglRobotics-forks/kuka_experimental/tree/foxy) | [![Foxy Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy)
[![Foxy Source Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-source-build.yml?branch=foxy) | [![Doxygen Doc Deployment](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml/badge.svg)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml)
[Generated Doc](https://StoglRobotics-forks.github.io/kuka_experimental_Documentation/foxy/html/index.html) | [kuka_experimental](https://index.ros.org/p/kuka_experimental/#foxy) + + +### Explanation of different build types + +**NOTE**: There are three build stages checking current and future compatibility of the package. + +[Detailed build status](.github/workflows/README.md) + +1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that direct local build is possible. + + Uses repos file: `$NAME$-not-released..repos` + +1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. + Shows that local build with dependencies is possible and if fails there we can expect that after the next package sync we will not be able to build. + + Uses repos file: `$NAME$.repos` + +1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml new file mode 100644 index 000000000..2d0286518 --- /dev/null +++ b/.github/workflows/ci-format.yml @@ -0,0 +1,23 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4.4.0 + with: + python-version: '3.10' + - name: Install system hooks + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml new file mode 100644 index 000000000..0e98f5007 --- /dev/null +++ b/.github/workflows/ci-ros-lint.yml @@ -0,0 +1,60 @@ +name: ROS Lint +on: + pull_request: + +jobs: + ament_lint: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cppcheck, copyright, lint_cmake] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: ${{ matrix.linter }} + package-name: + kuka_resources + kuka_lbr_iiwa_support + kuka_kr210_support + kuka_kr150_support + kuka_kr120_support + kuka_kr16_support + kuka_kr10_support + kuka_kr6_support + kuka_kr5_support + kuka_kr3_support + kuka_experimental + + + ament_lint_100: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cpplint] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: cpplint + arguments: "--linelength=100 --filter=-whitespace/newline" + package-name: + kuka_resources + kuka_lbr_iiwa_support + kuka_kr210_support + kuka_kr150_support + kuka_kr120_support + kuka_kr16_support + kuka_kr10_support + kuka_kr6_support + kuka_kr5_support + kuka_kr3_support + kuka_experimental diff --git a/.github/workflows/ci_bionic.yml b/.github/workflows/ci_bionic.yml new file mode 100644 index 000000000..0c6c68334 --- /dev/null +++ b/.github/workflows/ci_bionic.yml @@ -0,0 +1,49 @@ +name: CI - Ubuntu Bionic + +on: + # direct pushes to protected branches are not supported + pull_request: + branches: + - melodic-devel + # run every day, at 6am UTC + schedule: + - cron: '0 6 * * *' + # allow manually starting this workflow + workflow_dispatch: + +jobs: + industrial_ci: + name: ROS Melodic (${{ matrix.ros_repo }}) + runs-on: ubuntu-20.04 + + strategy: + fail-fast: false + matrix: + ros_distro: [ melodic ] + ros_repo: [ main, testing ] + + env: + CCACHE_DIR: "${{ github.workspace }}/.ccache" + CATKIN_LINT: true + + steps: + - name: Fetch repository + uses: actions/checkout@v3 + + - name: ccache cache + uses: actions/cache@v3 + with: + path: ${{ env.CCACHE_DIR }} + # we always want the ccache cache to be persisted, as we cannot easily + # determine whether dependencies have changed, and ccache will manage + # updating the cache for us. Adding 'run_id' to the key will force an + # upload at the end of the job. + key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} + restore-keys: | + ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} + + - name: Run industrial_ci + uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{ matrix.ros_distro }} + ROS_REPO: ${{ matrix.ros_repo }} diff --git a/.github/workflows/ci_focal.yml b/.github/workflows/ci_focal.yml new file mode 100644 index 000000000..319e5e600 --- /dev/null +++ b/.github/workflows/ci_focal.yml @@ -0,0 +1,49 @@ +name: CI - Ubuntu Focal + +on: + # direct pushes to protected branches are not supported + pull_request: + branches: + - melodic-devel + # run every day, at 6am UTC + schedule: + - cron: '0 6 * * *' + # allow manually starting this workflow + workflow_dispatch: + +jobs: + industrial_ci: + name: ROS Noetic (${{ matrix.ros_repo }}) + runs-on: ubuntu-20.04 + + strategy: + fail-fast: false + matrix: + ros_distro: [ noetic ] + ros_repo: [ main, testing ] + + env: + CCACHE_DIR: "${{ github.workspace }}/.ccache" + CATKIN_LINT: true + + steps: + - name: Fetch repository + uses: actions/checkout@v3 + + - name: ccache cache + uses: actions/cache@v3 + with: + path: ${{ env.CCACHE_DIR }} + # we always want the ccache cache to be persisted, as we cannot easily + # determine whether dependencies have changed, and ccache will manage + # updating the cache for us. Adding 'run_id' to the key will force an + # upload at the end of the job. + key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} + restore-keys: | + ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} + + - name: Run industrial_ci + uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{ matrix.ros_distro }} + ROS_REPO: ${{ matrix.ros_repo }} diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml new file mode 100644 index 000000000..7ce17effd --- /dev/null +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Foxy - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: foxy + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build-main.yml b/.github/workflows/foxy-binary-build-main.yml new file mode 100644 index 000000000..713479b12 --- /dev/null +++ b/.github/workflows/foxy-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Foxy Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: main + upstream_workspace: kuka_experimental-not-released.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml new file mode 100644 index 000000000..472c4c5e1 --- /dev/null +++ b/.github/workflows/foxy-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Foxy Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: testing + upstream_workspace: kuka_experimental-not-released.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-main.yml b/.github/workflows/foxy-semi-binary-build-main.yml new file mode 100644 index 000000000..350a4ef94 --- /dev/null +++ b/.github/workflows/foxy-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Foxy Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: main + upstream_workspace: kuka_experimental.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml new file mode 100644 index 000000000..69fbec474 --- /dev/null +++ b/.github/workflows/foxy-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Foxy Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: testing + upstream_workspace: kuka_experimental.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-source-build.yml b/.github/workflows/foxy-source-build.yml new file mode 100644 index 000000000..de4ca93dc --- /dev/null +++ b/.github/workflows/foxy-source-build.yml @@ -0,0 +1,19 @@ +name: Foxy Source Build +on: + workflow_dispatch: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + with: + ros_distro: foxy + ref: foxy + ros2_repo_branch: foxy diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml new file mode 100644 index 000000000..b9460bda4 --- /dev/null +++ b/.github/workflows/prerelease-check.yml @@ -0,0 +1,39 @@ +name: Pre-Release Check + +on: + workflow_dispatch: + inputs: + ros_distro: + description: 'Chose ROS distribution' + required: true + default: 'rolling' + type: choice + options: + - foxy + - galactic + - humble + - rolling + branch: + description: 'Chose branch for distro' + required: true + default: 'master' + type: choice + options: + - foxy + - galactic + - humble + - master + +jobs: + pre_release: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + ref: ${{ github.event.inputs.branch }} + - name: industrial_ci + uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{ github.event.inputs.ros_distro }} + PRERELEASE: true + BASEDIR: ${{ github.workspace }}/.work diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml new file mode 100644 index 000000000..490b680e7 --- /dev/null +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -0,0 +1,96 @@ +name: Reusable industrial_ci Workflow with Cache +# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache +# author: Denis Štogl + +on: + workflow_call: + inputs: + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + + upstream_workspace: + description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' + required: true + type: string + ros_distro: + description: 'ROS_DISTRO variable for industrial_ci' + required: true + type: string + ros_repo: + description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' + default: 'main' + required: false + type: string + os_code_name: + description: 'OS_CODE_NAME variable for industrial_ci' + default: '' + required: false + type: string + before_install_upstream_dependencies: + description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' + default: '' + required: false + type: string + + ccache_dir: + description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.ccache' + required: false + type: string + basedir: + description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.work' + required: false + type: string + + +jobs: + reusable_industrial_ci_with_cache: + name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} + runs-on: ubuntu-latest + env: + CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} + BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} + steps: + - name: Checkout ${{ inputs.ref }} when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v3 + - name: Checkout ${{ inputs.ref }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v3 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + - name: cache target_ws + if: ${{ ! matrix.env.CCOV }} + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - uses: 'ros-industrial/industrial_ci@master' + env: + UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} + ROS_DISTRO: ${{ inputs.ros_distro }} + ROS_REPO: ${{ inputs.ros_repo }} + OS_CODE_NAME: ${{ inputs.os_code_name }} + BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} + - name: prepare target_ws for cache + if: ${{ always() && ! matrix.env.CCOV }} + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml new file mode 100644 index 000000000..7773730d3 --- /dev/null +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -0,0 +1,59 @@ +name: Reusable industrial_ci Workflow with Cache +# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache +# author: Denis Štogl + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref: + description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' + required: true + type: string + ros2_repo_branch: + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + default: 'master' + required: false + type: string + +jobs: + reusable_ros_tooling_source_build: + name: ${{ inputs.ros_distro }} ubuntu-22.04 + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + steps: + - uses: ros-tooling/setup-ros@0.3.4 + with: + required-ros-distributions: ${{ inputs.ros_distro }} + - uses: actions/checkout@v3 + with: + ref: ${{ inputs.ref }} + - uses: ros-tooling/action-ros-ci@0.2.6 + with: + target-ros2-distro: ${{ inputs.ros_distro }} + # build all packages listed in the meta package + package-name: + kuka_resources + kuka_lbr_iiwa_support + kuka_kr210_support + kuka_kr150_support + kuka_kr120_support + kuka_kr16_support + kuka_kr10_support + kuka_kr6_support + kuka_kr5_support + kuka_kr3_support + kuka_experimental + + vcs-repo-file-url: | + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/kuka_experimental.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - uses: actions/upload-artifact@v1 + with: + name: colcon-logs-ubuntu-22.04 + path: ros_ws/log diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml new file mode 100644 index 000000000..dc3398f66 --- /dev/null +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Rolling - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - rolling + pull_request: + branches: + - rolling + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: rolling + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml new file mode 100644 index 000000000..a7e13dc3c --- /dev/null +++ b/.github/workflows/rolling-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Rolling Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - rolling + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: main + upstream_workspace: kuka_experimental-not-released.rolling.repos + ref_for_scheduled_build: rolling diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml new file mode 100644 index 000000000..90b9494c6 --- /dev/null +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Rolling Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - rolling + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: kuka_experimental-not-released.rolling.repos + ref_for_scheduled_build: rolling diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml new file mode 100644 index 000000000..798ccc141 --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Rolling Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - rolling + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: main + upstream_workspace: kuka_experimental.rolling.repos + ref_for_scheduled_build: rolling diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml new file mode 100644 index 000000000..73c24a4db --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Rolling Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - rolling + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: kuka_experimental.rolling.repos + ref_for_scheduled_build: rolling diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml new file mode 100644 index 000000000..3da2d9af7 --- /dev/null +++ b/.github/workflows/rolling-source-build.yml @@ -0,0 +1,19 @@ +name: Rolling Source Build +on: + workflow_dispatch: + branches: + - rolling + push: + branches: + - rolling + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + with: + ros_distro: rolling + ref: rolling + ros2_repo_branch: rolling diff --git a/.gitignore b/.gitignore index 7b171072a..a9214a4b2 100644 --- a/.gitignore +++ b/.gitignore @@ -16,6 +16,7 @@ msg/_.*\.py$ \.pcd$ .pyc$ +**/__pycache__/ # Generated by dynamic reconfigure \.cfgc$ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..ea4786e51 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,144 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + exclude: \.(dae|DAE)$ + - id: mixed-line-ending + - id: trailing-whitespace + - id: fix-byte-order-marker + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v3.3.1 + hooks: + - id: pyupgrade + args: [--py36-plus] + + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + args: ["--line-length=99"] + + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.3.0 + hooks: + - id: pydocstyle + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + + - repo: https://github.com/pycqa/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + args: ["--ignore=E501"] + + # CPP hooks + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-14 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + # The same options as in ament_cppcheck are used, but its not working... + #- repo: https://github.com/pocc/pre-commit-hooks + #rev: v1.1.1 + #hooks: + #- id: cppcheck + #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-whitespace/newline"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + # Copyright + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.10.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.2.2 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words-list=linz'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 6a00b67cd..000000000 --- a/.travis.yml +++ /dev/null @@ -1,25 +0,0 @@ -sudo: required -dist: trusty -language: generic - -compiler: - - gcc - -notifications: - email: - on_failure: always - -env: - matrix: - - USE_DEB=true - ROS_DISTRO="indigo" - ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true - ROS_DISTRO="indigo" - ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu - -install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config - -script: - - source .ci_config/travis.sh diff --git a/LICENSE b/LICENSE index ad410e113..29d1b8eca 100644 --- a/LICENSE +++ b/LICENSE @@ -186,16 +186,16 @@ Apache License same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright {yyyy} {name of copyright owner} + Copyright {yyyy} {name of copyright owner} - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 + http://www.apache.org/licenses/LICENSE-2.0 - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. \ No newline at end of file + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md index 718da785a..6d4b9c46e 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,201 @@ -# Kuka experimental -[![Build Status](http://build.ros.org/job/Kdev__kuka_experimental__ubuntu_xenial_amd64/badge/icon)](http://build.ros.org/job/Kdev__kuka_experimental__ubuntu_xenial_amd64) +# Kuka experimental -[![support level: community](https://img.shields.io/badge/support%20level-community-lightgray.png)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform) +[![support level: community](https://img.shields.io/badge/support%20level-community-lightgray.svg)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform) Experimental packages for Kuka manipulators within [ROS-Industrial][]. See the [ROS wiki][] page for more information. - -## Contents - This repository contains packages that will be migrated to the [kuka][] -repository after they have received sufficient testing. The contents of -these packages are subject to change, without prior notice. Any available -APIs are to be considered unstable and are not guaranteed to be complete +repository after they have received sufficient testing. The contents of +these packages are subject to change, without prior notice. Any available +APIs are to be considered unstable and are not guaranteed to be complete and / or functional. - [ROS-Industrial]: http://wiki.ros.org/Industrial [ROS wiki]: http://wiki.ros.org/kuka_experimental [kuka]: https://github.com/ros-industrial/kuka + +## Contents + +KUKA is one the major robot manufacturers in the world. Therefore, KUKA robots can be found in many industrial applications and research labs. + +This branch contains ROS 2 version of the driver supporting new features enabled by ROS 2, such as decreased latency, improved security, and more flexibility regarding middleware configuration. +The repository contains descriptions for many KUKA manipulators, ros2_control support for RSI and EKI Protocols to control the robot, as well as generic MoveIt2 setup to get you started. + +[//]: <> (TODO: I you want to learn more check the videos and presentation about the repository:) + + +## Build status + +ROS2 Distro | Branch | Build status | Documentation | Released packages +:---------: | :----: | :----------: | :-----------: | :---------------: +**Rolling** | [`rolling`](https://github.com/StoglRobotics-forks/kuka_experimental/tree/rolling) | [![Rolling Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-binary-build-main.yml?branch=rolling)
[![Rolling Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=rolling)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/rolling-semi-binary-build-main.yml?branch=rolling) | [![Doxygen Doc Deployment](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml/badge.svg)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml)
[Generated Doc](https://StoglRobotics-forks.github.io/kuka_experimental_Documentation/rolling/html/index.html) | [kuka_experimental](https://index.ros.org/p/kuka_experimental/#rolling) +**Foxy** | [`foxy`](https://github.com/StoglRobotics-forks/kuka_experimental/tree/foxy) | [![Foxy Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [![Doxygen Doc Deployment](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml/badge.svg)](https://github.com/StoglRobotics-forks/kuka_experimental/actions/workflows/doxygen-deploy.yml)
[Generated Doc](https://StoglRobotics-forks.github.io/kuka_experimental_Documentation/foxy/html/index.html) | [kuka_experimental](https://index.ros.org/p/kuka_experimental/#foxy) + +### Explanation of different build types + +**NOTE**: There are three build stages checking current and future compatibility of the package. + +[Detailed build status](.github/workflows/README.md) + +1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that direct local build is possible. + + Uses repos file: `$NAME$-not-released..repos` + +1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. + Shows that local build with dependencies is possible and if fails there we can expect that after the next package sync we will not be able to build. + + Uses repos file: `$NAME$.repos` + +1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. + +## Packages in the Repository: + +- `kuka_experimental` - Meta package that provides a single point of installation for the released packages. +- `kuka_eki_hw_interface` - Hardware interface implementation for ros2_control based on EKI communication protocol. +- `kuka_rsi_hw_interface` - Hardware interface implementation for ros2_control based on RSI communication protocol. +- `kuka_resources` - Shared resources across support packages, e.g., rviz configuration, launch files, and URDF properties (colors, materials, math constants), etc. +- `kuka_ros2_control_support` - Support files for using KUKA with ros2_control. +- `kuka_rsi_simulator` - Simple simulator of RSI communication that is useful for driver testing without connected hardware. + +- **support packages** - descriptions, meshes and test-launch file for different versions of KUKA manipulators. + + - `kuka_kr3_support` + - `kuka_kr5_support` + - `kuka_kr6_support` + - `kuka_kr10_support` + - `kuka_kr16_support` + - `kuka_kr120_support` + - `kuka_kr150_support` + - `kuka_kr210_support` + - `kuka_lbr_iiwa_support` - *NOT tested extensively! Note that there are packages with possibly better support!* + + +## Getting Started + +For getting started to use this driver you have to follow the next steps: + +1. **Install the driver** (see below). Currently only install from source is supported. + +2. **Setup the robot**. Once driver is installed, decide which interface, RSI or EKI, you want to use and [check instructions for RSI interface](./kuka_rsi_hw_interface/krl/README.md) or [check instructions for EKI interface](./kuka_eki_hw_interface/krl/README.md). + +3. **Start the driver** (see below). + + +### Build from Source + +Before building from source please make sure that you actually need to do that. Building from source +might require some special treatment, especially when it comes to dependency management. +Dependencies might change from time to time. Upstream packages might change +their features / API which require changes in this repo. Therefore, this repo's source builds might +require upstream repositories to be present in a certain version as otherwise builds might fail. +Starting from scratch following exactly the steps below should always work, but simply pulling and +building might fail occasionally. + +1. [Install ROS2](https://docs.ros.org/en/rolling/Installation/Ubuntu-Install-Debians.html). + For using this driver with ROS2 `foxy`. Checkout [foxy + branch](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/foxy), for using it + with ROS2 ``humble``, use the [humble branch](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/humble), + and for using it with ROS2 ``rolling``, use the [rolling + branch](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/rolling). + + Once installed, please make sure to actually [source ROS2](https://docs.ros.org/en/rolling/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html#source-the-setup-files) before proceeding. + +3. Make sure that `colcon`, its extensions and `vcs` are installed: + ``` + sudo apt install python3-colcon-common-extensions python3-vcstool + ``` + +4. Create a new ROS2 workspace: + ``` + export COLCON_WS=~/workspace/ros2_kuka_driver + mkdir -p $COLCON_WS/src + ``` + +5. Clone relevant packages, install dependencies, compile, and source the workspace by using: + ``` + cd $COLCON_WS + git clone https://github.com/StoglRobotics-forks/kuka_experimental.git src/kuka_experimental + vcs import src --skip-existing --input src/kuka_experimental/kuka_experimental-not-released.${ROS_DISTRO}.repos + rosdep update + rosdep install --ignore-src --from-paths src -y + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + source install/setup.bash + ``` + +6. When consecutive pulls leads to build errors, please make sure to update the upstream packages before + filing an issue: + ``` + cd $COLCON_WS + vcs import src --skip-existing --input src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver-not-released.${ROS_DISTRO}.repos + rosdep update + rosdep install --ignore-src --from-paths src -y + ``` + + +### Starting the driver + +1. Start *Mock Hardware* first to see how the system is setup (example for `KR5 arc`): + ``` + ros2 launch kuka_ros2_control_support test_bringup.launch.py description_package:=kuka_kr5_support description_macro_file:=kr5_arc_macro.xacro use_mock_hardware:=true + ``` + Now check started nodes, available topics and services. + +2. Start test node for joint trajectory controller: + ``` + ros2 launch kuka_ros2_control_support test_joint_trajectory_controller.launch.py + ``` + +3. To start other robot type use the following arguments: + ``` + "description_package", + choices=[ + "kuka_kr3_support", + "kuka_kr5_support", + "kuka_kr6_support", + "kuka_kr10_support", + "kuka_kr16_support", + "kuka_kr120_support", + "kuka_kr150_support", + "kuka_kr210_support", + "kuka_lbr_iiwa_support", + ] + + "description_macro_file", + choices=[ + "kr3r540_macro.xacro", + "kr5_arc_macro.xacro", + "kr6r700sixx_macro.xacro", + "kr6r900_2_macro.xacro", + "kr6r900sixx_macro.xacro", + "kr10r900_2_macro.xacro", + "kr10r1100sixx_macro.xacro", + "kr10r1420_macro.xacro", + "kr16_2_macro.xacro", + "kr120r2500pro_macro.xacro", + "kr150_2_macro.xacro", + "kr150r3100_2_macro.xacro", + "kr210l150_macro.xacro", + "lbr_iiwa_14_r820_macro.xacro", + ] + + "controllers_file", + choices=[ + "kuka_6dof_controllers.yaml", + "kuka_7dof_controllers.yaml", + # Note: for the robot kuka_lbr_iiwa_14_r820, kuka_7dof_controllers.yaml should be used + # and the rest use kuka_6dof_controllers.yaml + ] + + ``` + **NOTE: Please choose only the related combination of the various parameters.** + +4. Start the real hardware using the following arguments on `test_bringup.launch.py`: + ``` + use_rsi_communication:=true rsi_listen_ip:= rsi_listen_port:=49152 + ``` + You can omit `rsi_listen_port` if you didn't change it in the robots configuration. + For example: + ``` + ``` diff --git a/kuka_common_moveit/CMakeLists.txt b/kuka_common_moveit/CMakeLists.txt new file mode 100644 index 000000000..244e68eb1 --- /dev/null +++ b/kuka_common_moveit/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.16) + +project(kuka_common_moveit LANGUAGES CXX) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY config launch srdf rviz + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/kuka_common_moveit/config/chomp_planning.yaml b/kuka_common_moveit/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/kuka_common_moveit/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/kuka_common_moveit/config/fake_controllers.yaml b/kuka_common_moveit/config/fake_controllers.yaml new file mode 100644 index 000000000..a3cd3a2e3 --- /dev/null +++ b/kuka_common_moveit/config/fake_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: fake_manipulator_controller + type: $(arg execution_type) + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/kuka_common_moveit/config/joint_limits.yaml b/kuka_common_moveit/config/joint_limits.yaml new file mode 100644 index 000000000..6926d284f --- /dev/null +++ b/kuka_common_moveit/config/joint_limits.yaml @@ -0,0 +1,49 @@ +/**: + ros__parameters: + robot_description_planning: + # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + + # For beginners, we downscale velocity and acceleration limits. + # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. + default_velocity_scaling_factor: 0.1 + default_acceleration_scaling_factor: 0.1 + + # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] + # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + joint_limits: + joint_a1: + has_velocity_limits: true + max_velocity: 1.832596 + has_acceleration_limits: true + max_acceleration: 3.0 + max_deceleration: 3.0 + joint_a2: + has_velocity_limits: true + max_velocity: 1.640609 + has_acceleration_limits: true + max_acceleration: 5.0 + max_deceleration: 5.0 + joint_a3: + has_velocity_limits: true + max_velocity: 1.745329 + has_acceleration_limits: true + max_acceleration: 5.0 + max_deceleration: 5.0 + joint_a4: + has_velocity_limits: true + max_velocity: 2.373648 + has_acceleration_limits: true + max_acceleration: 3.0 + max_deceleration: 3.0 + joint_a5: + has_velocity_limits: true + max_velocity: 2.251475 + has_acceleration_limits: true + max_acceleration: 5.0 + max_deceleration: 5.0 + joint_a6: + has_velocity_limits: true + max_velocity: 3.595378 + has_acceleration_limits: true + max_acceleration: 7.0 + max_deceleration: 7.0 \ No newline at end of file diff --git a/kuka_common_moveit/config/kinematics.yaml b/kuka_common_moveit/config/kinematics.yaml new file mode 100644 index 000000000..b8eef68f9 --- /dev/null +++ b/kuka_common_moveit/config/kinematics.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + robot_description_kinematics: + manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/kuka_common_moveit/config/kuka_controllers.yaml b/kuka_common_moveit/config/kuka_controllers.yaml new file mode 100644 index 000000000..0ea9d7f15 --- /dev/null +++ b/kuka_common_moveit/config/kuka_controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - position_trajectory_controller + +position_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + diff --git a/kuka_common_moveit/config/kuka_servo_config.yaml b/kuka_common_moveit/config/kuka_servo_config.yaml new file mode 100644 index 000000000..ecfb05c82 --- /dev/null +++ b/kuka_common_moveit/config/kuka_servo_config.yaml @@ -0,0 +1,78 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + +## MoveIt properties +move_group_name: manipulator # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Collision checking begins slowing down when nearer than a specified distance. +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] + +## Incoming Joint State properties +low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. + +collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] +collision_check_type: threshold_distance \ No newline at end of file diff --git a/kuka_common_moveit/config/move_group_config.yaml b/kuka_common_moveit/config/move_group_config.yaml new file mode 100644 index 000000000..3ae63c91f --- /dev/null +++ b/kuka_common_moveit/config/move_group_config.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + planning_pipelines: ["ompl", "pilz", "stomp"] + + capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService + + # Trajectory execution manager parameters + allow_trajectory_execution: true + moveit_manage_controllers: false + trajectory_execution: + allowed_execution_duration_scaling: 100.0 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + # Planning scene monitor parameters + publish_planning_scene: true + publish_geometry_updates: true + publish_state_updates: true + publish_transforms_updates: true \ No newline at end of file diff --git a/kuka_common_moveit/config/moveit.rviz b/kuka_common_moveit/config/moveit.rviz new file mode 100644 index 000000000..c8d63d091 --- /dev/null +++ b/kuka_common_moveit/config/moveit.rviz @@ -0,0 +1,376 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 337 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cell_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + roof: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + wall_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cell_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + roof: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + wall_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wall_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.6461073160171509 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.09409921616315842 + Y: 0.0935380682349205 + Z: 0.1795470118522644 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4303983449935913 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.93857479095459 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/kuka_common_moveit/config/moveit_controllers.yaml b/kuka_common_moveit/config/moveit_controllers.yaml new file mode 100644 index 000000000..8ea79a60e --- /dev/null +++ b/kuka_common_moveit/config/moveit_controllers.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_manage_controllers: false + moveit_simple_controller_manager: + controller_names: + - position_trajectory_controller + + position_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + trajectory_execution: + allowed_execution_duration_scaling: 100.0 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + ompl: + manipulator: + projection_evaluator: joints(joint_a1,joint_a2) + + publish_geometry_updates: true + publish_planning_scene: true + publish_state_updates: true + publish_transforms_updates: true diff --git a/kuka_common_moveit/config/ompl_planning.yaml b/kuka_common_moveit/config/ompl_planning.yaml new file mode 100644 index 000000000..962a215ef --- /dev/null +++ b/kuka_common_moveit/config/ompl_planning.yaml @@ -0,0 +1,166 @@ +/**: + ros__parameters: + ompl: + planning_plugin: ompl_interface/OMPLPlanner + request_adapters: default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStatePathConstraints + start_state_max_bounds_error: 0.1 + planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + longest_valid_segment_fraction: 0.5 + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + manipulator: + projection_evaluator: joints(joint_a1,joint_a2) + longest_valid_segment_fraction: 0.005 + default_planner_config: RRTConnect + default_workspace_bounds: 100 + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/kuka_common_moveit/config/pilz_capabilities.yaml b/kuka_common_moveit/config/pilz_capabilities.yaml new file mode 100644 index 000000000..2ddfe5aab --- /dev/null +++ b/kuka_common_moveit/config/pilz_capabilities.yaml @@ -0,0 +1,3 @@ +capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService \ No newline at end of file diff --git a/kuka_common_moveit/config/pilz_cartesian_limits.yaml b/kuka_common_moveit/config/pilz_cartesian_limits.yaml new file mode 100644 index 000000000..8e6105663 --- /dev/null +++ b/kuka_common_moveit/config/pilz_cartesian_limits.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + robot_description_planning: + cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 \ No newline at end of file diff --git a/kuka_common_moveit/config/pilz_planning.yaml b/kuka_common_moveit/config/pilz_planning.yaml new file mode 100644 index 000000000..86e2957ce --- /dev/null +++ b/kuka_common_moveit/config/pilz_planning.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + pilz: + planning_plugin: pilz_industrial_motion_planner/CommandPlanner + default_planner_config: PTP + request_adapters: default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision + response_adapters: default_planning_response_adapters/AddTimeOptimalParameterization default_planning_response_adapters/ValidateSolution default_planning_response_adapters/DisplayMotionPath diff --git a/kuka_common_moveit/config/ros_controllers.yaml b/kuka_common_moveit/config/ros_controllers.yaml new file mode 100644 index 000000000..ca3a7be2a --- /dev/null +++ b/kuka_common_moveit/config/ros_controllers.yaml @@ -0,0 +1,35 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: todo_group_name + joint_model_group_pose: todo_state_name +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: position_trajectory_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 \ No newline at end of file diff --git a/kuka_common_moveit/config/sensors_3d.yaml b/kuka_common_moveit/config/sensors_3d.yaml new file mode 100644 index 000000000..d2955dcd9 --- /dev/null +++ b/kuka_common_moveit/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/kuka_common_moveit/config/stomp_planning.yaml b/kuka_common_moveit/config/stomp_planning.yaml new file mode 100644 index 000000000..40e6f8091 --- /dev/null +++ b/kuka_common_moveit/config/stomp_planning.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + stomp: + planning_plugins: [stomp_moveit/StompPlanner] + request_adapters: "default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision" + response_adapters: "default_planning_response_adapters/AddTimeOptimalParameterization default_planning_response_adapters/ValidateSolution default_planning_response_adapters/DisplayMotionPath" + + stomp_moveit: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + exponentiated_cost_sensitivity: 0.5 + control_cost_weight: 0.1 + delta_t: 0.1 \ No newline at end of file diff --git a/kuka_common_moveit/launch/bringup.launch.py b/kuka_common_moveit/launch/bringup.launch.py new file mode 100644 index 000000000..6f0dc2bc8 --- /dev/null +++ b/kuka_common_moveit/launch/bringup.launch.py @@ -0,0 +1,528 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import yaml +import sys +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import xacro +from launch_ros.substitutions import FindPackageShare + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_name", + default_value="kuka", + description="Name of the robot or application for unique identification.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value="", + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_macro_file", + description="URDF/XACRO description file with of the robot or application. \ + The expected location of the file is '/urdf/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "configuration_package", + default_value="kuka_resources", + description="Package with configuration files, e.g., controllers, initial positions.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="kuka_6dof_controllers.yaml", + description="YAML file with the controllers configuration. \ + The expected location of the file is '/config/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_positions_file", + default_value="initial_positions.yaml", + description="YAML file with the initial positions when using mock hardware from \ + ros2_control. The expected location of the file is '/config/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "joint_limits_file", + default_value="joint_limits.yaml", + description="YAML file with the joint limits for the robot cell.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_eki_communication", + default_value="false", + description="Use EKI communication to the KUKA Robot Controller (KR-C). \ + If the flag is set to true 'eki_robot_ip' and 'eki_robot_port' arguments define the \ + endpoint of robot controller to connect.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_ip", + default_value="127.0.0.1", + description="IP address for the robot controller for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_port", + default_value="54600", + description="Port by which the robot can be reached for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rsi_communication", + default_value="false", + description="Use RSI communication to the KUKA Robot Controller (KR-C). \ + If the flag is set to true 'rsi_listen_ip' and 'rsi_robot_port' arguments define the \ + endpoint where robot controller should connect to. (Keep in mind that you have to \ + define this endpoint also in the RSI configuration of the program.)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_ip", + default_value="127.0.0.1", + description="IP address of a computer where robot controller should connect for \ + communication using RSI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_port", + default_value="49152", + description="Port on a computer where robot controller should connect for \ + communication using RSI protocol.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "hardware_is_async", + default_value="true", + description="Set the hardware to be asynchronos to the controller manager.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + default_value="kuka_common_moveit", + description="Package with semantic robot description needed for MoveIt2.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "semantic_description_file", + default_value="common_kuka.srdf.xacro", + description="Semantic robot description file (SRDF/XACRO) with of the robot or \ + application. The expected location of the file is '/srdf/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rviz_file", + default_value="moveit.rviz", + description="Rviz2 configuration file of the visualization. \ + The expected location of the file is '/rviz/'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "activate_ros2_control", + default_value="true", + description="Decide if this file should also start ros2_control stack and activate\ + controllers. This is useful for testing, but nor advised in production." + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "additional_xacro_args", + default_value="true", + description="additional_xacro_args" + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_external_gripper_driver", + default_value="true", + description="Disable starting of local gripper driver. Use driver running on CtrlX device.", + ) + ) + + # initialize arguments + robot_name = LaunchConfiguration("robot_name") + prefix = LaunchConfiguration("prefix") + description_package = LaunchConfiguration("description_package") + description_macro_file = LaunchConfiguration("description_macro_file") + configuration_package = LaunchConfiguration("configuration_package") + controllers_file = LaunchConfiguration("controllers_file") + initial_positions_file = LaunchConfiguration("initial_positions_file") + joint_limits_file = LaunchConfiguration("joint_limits_file") + + use_eki_communication = LaunchConfiguration("use_eki_communication") + eki_robot_ip = LaunchConfiguration("eki_robot_ip") + eki_robot_port = LaunchConfiguration("eki_robot_port") + use_rsi_communication = LaunchConfiguration("use_rsi_communication") + rsi_listen_ip = LaunchConfiguration("rsi_listen_ip") + rsi_listen_port = LaunchConfiguration("rsi_listen_port") + hardware_is_async = LaunchConfiguration("hardware_is_async") + + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + use_external_gripper_driver = LaunchConfiguration("use_external_gripper_driver") + + moveit_config_package = LaunchConfiguration("moveit_config_package") + semantic_description_file = LaunchConfiguration("semantic_description_file") + rviz_file = LaunchConfiguration("rviz_file") + + activate_ros2_control = LaunchConfiguration("activate_ros2_control") + + additional_xacro_args = LaunchConfiguration("additional_xacro_args") + + initial_positions_file = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", initial_positions_file] + ) + joint_limits_file = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", joint_limits_file] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("kuka_ros2_control_support"), "urdf", "common_kuka.xacro"] + ), + " ", + "robot_name:=", + robot_name, + " ", + "prefix:=", + prefix, + " ", + "description_package:=", + description_package, + " ", + "description_macro_file:=", + description_macro_file, + " ", + "use_eki_communication:=", + use_eki_communication, + " ", + "eki_robot_ip:=", + eki_robot_ip, + " ", + "eki_robot_port:=", + eki_robot_port, + " ", + "use_rsi_communication:=", + use_rsi_communication, + " ", + "rsi_listen_ip:=", + rsi_listen_ip, + " ", + "rsi_listen_port:=", + rsi_listen_port, + " ", + "is_async:=", + hardware_is_async, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "initial_positions_file:=", + initial_positions_file, + " ", + "activate_ros2_control:=", + activate_ros2_control, + " ", + "additional_xacro_args:=", + additional_xacro_args, + " ", + "use_external_gripper_driver:=", + use_external_gripper_driver, + + ] + ) + robot_description = {"robot_description": robot_description_content} + + # Publish TF + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher_node", + output="both", + parameters=[robot_description] + ) + + # ros2_control_node + robot_controllers = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", controllers_file] + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output={ + "stdout": "screen", + "stderr": "screen", + }, + condition=IfCondition(activate_ros2_control), + ) + + # Spawn controllers + load_and_activate_controllers = [] + for controller in ["position_trajectory_controller", "joint_state_broadcaster"]: + load_and_activate_controllers += [ + ExecuteProcess( + cmd=[f"ros2 run controller_manager spawner {controller} -c controller_manager -p /home/oguz/maurobot_ws/install/kuka_ros2_control_support/share/kuka_ros2_control_support/config/6dof_controllers.yaml"], + shell=True, + output="screen", + condition=IfCondition(use_mock_hardware), + ) + ] + + # Spawn gripper controllers - only if mock hardware + load_and_activate_gripper_controllers = [] + for controller in ["gripper", "mocked_sensors_controller"]: + load_and_activate_gripper_controllers += [ + ExecuteProcess( + cmd=[f"ros2 run controller_manager spawner {controller}"], + shell=True, + output="screen", + condition=UnlessCondition(use_external_gripper_driver), + ) + ] + + # MoveIt2 Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "srdf", semantic_description_file] + ), + " ", + "robot_name:=", + robot_name, + " ", + "prefix:=", + prefix, + ] + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_content + } + + kinematics_yaml = load_yaml( + "kuka_common_moveit", "config/kinematics.yaml" + ) + + move_group_config = { + "planning_pipelines": ["ompl", "pilz", "stomp"], + "capabilities": [], + } + # Planning Functionality + # WARNING default_planner_request_adapters/FixStartStateCollision might cause jumps if the robot is in a slight collision at start, deactivating it for now + # see https://github.com/ros-planning/moveit/issues/2268 + ompl_planning_pipeline_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + + pilz_planning_pipeline_config = { + "pilz": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "default_planner_config": "PTP" + } + } + joint_limits_yaml = load_yaml( + "kuka_common_moveit", "config/joint_limits.yaml" + ) + ompl_planning_pipeline_config["ompl"].update(joint_limits_yaml) + + # WARNING default_planner_request_adapters/FixStartStateCollision might cause jumps if the robot is in a slight collision at start, deactivating it for now + # see https://github.com/ros-planning/moveit/issues/2268 + pilz_planning_yaml = load_yaml( + "kuka_common_moveit", "config/pilz_planning.yaml" + ) + pilz_planning_pipeline_config["pilz"].update(pilz_planning_yaml) + + pilz_capabilities_yaml = load_yaml( + "kuka_common_moveit", "config/pilz_capabilities.yaml" + ) + move_group_config.update(pilz_capabilities_yaml) + + pilz_limits_yaml = load_yaml( + "kuka_common_moveit", "config/pilz_cartesian_limits.yaml" + ) + robot_description_planning_config = { + "robot_description_planning" : pilz_limits_yaml + } + + stomp_planning_pipeline_config = { + "stomp": { + "planning_plugin": "stomp_moveit/StompPlanner", + } + } + + stomp_planning_yaml = load_yaml( + "kuka_common_moveit", "config/stomp_planning.yaml" + ) + stomp_planning_pipeline_config["stomp"].update(stomp_planning_yaml) + + + robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "kuka_common_moveit", "config/kuka_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + "moveit_manage_controllers": False, + "trajectory_execution.allowed_execution_duration_scaling": 100.0, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # MoveIt2 + moveit_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + move_group_config, + ompl_planning_pipeline_config, + pilz_planning_pipeline_config, + stomp_planning_pipeline_config, + robot_description_planning_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + + # Rviz2 + rviz_config = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "rviz", rviz_file]) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + #output="log", + output={ + 'stdout': 'log', + 'stderr': 'log' + }, + arguments=["-d", rviz_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + return LaunchDescription( + declared_arguments + [ + robot_state_publisher_node, + ros2_control_node, + moveit_node, + rviz_node, + ] + + load_and_activate_controllers + ) diff --git a/kuka_common_moveit/launch/bringup.launch.xml b/kuka_common_moveit/launch/bringup.launch.xml new file mode 100644 index 000000000..d7f9c8fdd --- /dev/null +++ b/kuka_common_moveit/launch/bringup.launch.xml @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_common_moveit/launch/test_bringup.launch.py b/kuka_common_moveit/launch/test_bringup.launch.py new file mode 100644 index 000000000..4fe7132e6 --- /dev/null +++ b/kuka_common_moveit/launch/test_bringup.launch.py @@ -0,0 +1,177 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="kuka_6dof_controllers.yaml", + choices=[ + "kuka_6dof_controllers.yaml", + "kuka_7dof_controllers.yaml", + ], + description="YAML file with the controllers configuration. Only LBR IIWA 14 has 7 dofs.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_positions_file", + default_value="initial_positions.yaml", + choices=[ + "initial_positions.yaml", + "initial_positions_all_zeros.yaml", + ], + description="YAML file with the initial positions when using mock hardware from \ + ros2_control. Robots using '_all_zeros' file are: \ + - kr210l150 \ + - " + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Description package with robot URDF/xacro files.", + choices=[ + "kuka_kr3_support", + "kuka_kr5_support", + "kuka_kr6_support", + "kuka_kr10_support", + "kuka_kr16_support", + "kuka_kr120_support", + "kuka_kr150_support", + "kuka_kr210_support", + "kuka_lbr_iiwa_support", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_macro_file", + description="URDF/XACRO description file with of the robot or application. \ + The expected location of the file is '/urdf/'.", + choices=[ + "kr3r540_macro.xacro", + "kr5_arc_macro.xacro", + "kr6r700sixx_macro.xacro", + "kr6r900_2_macro.xacro", + "kr6r900sixx_macro.xacro", + "kr10r900_2_macro.xacro", + "kr10r1100sixx_macro.xacro", + "kr10r1420_macro.xacro", + "kr16_2_macro.xacro", + "kr120r2500pro_macro.xacro", + "kr150_2_macro.xacro", + "kr150r3100_2_macro.xacro", + "kr210l150_macro.xacro", + "kr210r3100_macro.xacro", + "lbr_iiwa_14_r820_macro.xacro", + ], + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_eki_communication", + default_value="false", + description="Use EKI communication to the KUKA Robot Controller (KR-C). If the flag is set to true 'eki_robot_ip' and 'eki_robot_port' arguments define the endpoint of robot controller to connect.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_ip", + default_value="127.0.0.1", + description="IP address for the robot controller for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_port", + default_value="54600", + description="Port by which the robot can be reached for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rsi_communication", + default_value="false", + description="Use RSI communication to the KUKA Robot Controller (KR-C). If the flag is set to true 'rsi_listen_ip' and 'rsi_robot_port' arguments define the endpoint where robot controller should connect to. (Keep in mind that you have to define this endpoint also in the RSI configuration of the program.)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_ip", + default_value="127.0.0.1", + description="IP address of a computer where robot controller should connect for communication using RSI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_port", + default_value="49152", + description="Port on a computer where robot controller should connect for communication using RSI protocol.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + + # initialize arguments + controllers_file = LaunchConfiguration("controllers_file") + initial_positions_file = LaunchConfiguration("initial_positions_file") + description_package = LaunchConfiguration("description_package") + description_macro_file = LaunchConfiguration("description_macro_file") + + use_eki_communication = LaunchConfiguration("use_eki_communication") + eki_robot_ip = LaunchConfiguration("eki_robot_ip") + eki_robot_port = LaunchConfiguration("eki_robot_port") + use_rsi_communication = LaunchConfiguration("use_rsi_communication") + rsi_listen_ip = LaunchConfiguration("rsi_listen_ip") + rsi_listen_port = LaunchConfiguration("rsi_listen_port") + + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + + moveit_setup_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare('kuka_common_moveit'), 'launch', 'bringup.launch.py'])), + launch_arguments={ + "robot_name": "kuka_test_moveit_setup", + "controllers_file": controllers_file, + "initial_positions_file": initial_positions_file, + "description_package": description_package, + "description_macro_file": description_macro_file, + "use_eki_communication": use_eki_communication, + "eki_robot_ip": eki_robot_ip, + "eki_robot_port": eki_robot_port, + "use_rsi_communication": use_rsi_communication, + "rsi_listen_ip": rsi_listen_ip, + "rsi_listen_port": rsi_listen_port, + "use_mock_hardware": use_mock_hardware, + }.items(), + ) + + return LaunchDescription(declared_arguments + [moveit_setup_bringup_launch]) diff --git a/kuka_common_moveit/package.xml b/kuka_common_moveit/package.xml new file mode 100644 index 000000000..5be0369c3 --- /dev/null +++ b/kuka_common_moveit/package.xml @@ -0,0 +1,20 @@ + + + + kuka_common_moveit + 0.0.0 + TODO: Package description + robot + TODO: License declaration + + moveit + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/kuka_common_moveit/rviz/moveit.rviz b/kuka_common_moveit/rviz/moveit.rviz new file mode 100644 index 000000000..7446644a0 --- /dev/null +++ b/kuka_common_moveit/rviz/moveit.rviz @@ -0,0 +1,413 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 519 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Link1: + Alpha: 1 + Show Axes: false + Show Trail: false + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Link1: + Alpha: 1 + Show Axes: false + Show Trail: false + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Link1: + Alpha: 1 + Show Axes: false + Show Trail: false + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.182778358459473 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.09409921616315842 + Y: 0.0935380682349205 + Z: 0.1795470118522644 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4303983449935913 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.93857479095459 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 30 diff --git a/kuka_common_moveit/srdf/common_kuka.srdf.xacro b/kuka_common_moveit/srdf/common_kuka.srdf.xacro new file mode 100644 index 000000000..c4ee6ea7a --- /dev/null +++ b/kuka_common_moveit/srdf/common_kuka.srdf.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_eki_hw_interface/CMakeLists.txt b/kuka_eki_hw_interface/CMakeLists.txt index 3b6887483..43403e0a9 100644 --- a/kuka_eki_hw_interface/CMakeLists.txt +++ b/kuka_eki_hw_interface/CMakeLists.txt @@ -1,67 +1,87 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(kuka_eki_hw_interface) -add_compile_options(-std=c++11) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -find_package(catkin REQUIRED COMPONENTS - angles - cmake_modules - controller_manager - hardware_interface - joint_limits_interface - roscpp -) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED COMPONENTS system) +find_package(tinyxml_vendor REQUIRED) find_package(TinyXML REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(angles REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - kuka_eki_hw_interface - CATKIN_DEPENDS - controller_manager - hardware_interface - joint_limits_interface - roscpp - DEPENDS - TinyXML -) - +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -include_directories( +add_library( + ${PROJECT_NAME} + SHARED + src/kuka_eki_hw_interface.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${TinyXML_INCLUDE_DIRS} ) - -add_library(kuka_eki_hw_interface - src/kuka_eki_hw_interface.cpp +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + TinyXML + angles ) +pluginlib_export_plugin_description_file(hardware_interface ros2_control_kuka_eki.xml) -target_link_libraries(kuka_eki_hw_interface - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${TinyXML_LIBRARIES} +ament_export_include_directories( + include ) - -add_executable(kuka_eki_hw_interface_node - src/kuka_eki_hw_interface_node.cpp +ament_export_libraries( + ${PROJECT_NAME} ) - -target_link_libraries(kuka_eki_hw_interface_node - kuka_eki_hw_interface +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle ) +### ros2 control hardware plugin install( - TARGETS kuka_eki_hw_interface - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - + TARGETS ${PROJECT_NAME} + DESTINATION lib +) install( - TARGETS kuka_eki_hw_interface_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + DIRECTORY include/ + DESTINATION include +) +ament_package() diff --git a/kuka_eki_hw_interface/config/controller_joint_names.yaml b/kuka_eki_hw_interface/config/controller_joint_names.yaml deleted file mode 100644 index 5f8f0343a..000000000 --- a/kuka_eki_hw_interface/config/controller_joint_names.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] diff --git a/kuka_eki_hw_interface/config/hardware_controllers.yaml b/kuka_eki_hw_interface/config/hardware_controllers.yaml deleted file mode 100644 index 991bb3c90..000000000 --- a/kuka_eki_hw_interface/config/hardware_controllers.yaml +++ /dev/null @@ -1,18 +0,0 @@ -#Publish all joint states -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - -# Joint trajectory controller -position_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 - - state_publish_rate: 50 # Defaults to 50 - action_monitor_rate: 20 # Defaults to 20 diff --git a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h index 707ddc807..b1e43beea 100644 --- a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h @@ -1,106 +1,94 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 3M - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder, nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// Author: Brett Hemes (3M) - - -#ifndef KUKA_EKI_HW_INTERFACE -#define KUKA_EKI_HW_INTERFACE - -#include +// Copyright 2020 ros2_control Development Team +// Modifications copyright (c) 2021 Gergely Sóti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ +#define ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ + +#include #include - +#include #include - -#include -#include -#include -#include -#include - +#include +#include +#include "angles/angles.h" + +// ros2_control hardware_interface +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "tinyxml.h" +//#include "hardware_interface/system_interface.hpp" +//#include "hardware_interface/handle.hpp" +//#include "hardware_interface/hardware_info.hpp" +//#include "hardware_interface/system_interface.hpp" +//#include "hardware_interface/types/hardware_interface_return_values.hpp" +//#include "hardware_interface/types/lifecycle_state_names.hpp" +//#include "rclcpp/macros.hpp" +//#include "kuka_eki_hw_interface/visibility_control.h" + +// ROS +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" namespace kuka_eki_hw_interface { + class KukaEkiHardwareInterface : public hardware_interface::SystemInterface + { + public: + RCLCPP_SHARED_PTR_DEFINITIONS(KukaEkiHardwareInterface) + virtual ~KukaEkiHardwareInterface(); -class KukaEkiHardwareInterface : public hardware_interface::RobotHW -{ -private: - ros::NodeHandle nh_; - - const unsigned int n_dof_ = 6; - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - - // EKI - std::string eki_server_address_; - std::string eki_server_port_; - int eki_cmd_buff_len_; - int eki_max_cmd_buff_len_ = 5; // by default, limit command buffer to 5 (size of advance run in KRL) - - // Timing - ros::Duration control_period_; - ros::Duration elapsed_time_; - double loop_hz_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - - // EKI socket read/write - int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) - boost::asio::io_service ios_; - boost::asio::deadline_timer deadline_; - boost::asio::ip::udp::endpoint eki_server_endpoint_; - boost::asio::ip::udp::socket eki_server_socket_; - void eki_check_read_state_deadline(); - static void eki_handle_receive(const boost::system::error_code &ec, size_t length, - boost::system::error_code* out_ec, size_t* out_length); - bool eki_read_state(std::vector &joint_position, std::vector &joint_velocity, - std::vector &joint_effort, int &cmd_buff_len); - bool eki_write_command(const std::vector &joint_position); - -public: - - KukaEkiHardwareInterface(); - ~KukaEkiHardwareInterface(); - - void init(); - void start(); - void read(const ros::Time &time, const ros::Duration &period); - void write(const ros::Time &time, const ros::Duration &period); -}; - -} // namespace kuka_eki_hw_interface - -#endif // KUKA_EKI_HW_INTERFACE + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final; + + std::vector export_state_interfaces() final; + + std::vector export_command_interfaces() final; + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) final; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) final; + + hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) final; + hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) final; + + private: + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_states_; + + std::string eki_server_address_; + std::string eki_server_port_; + + int eki_cmd_buff_len_; + int eki_max_cmd_buff_len_ = 5; // by default, limit command buffer to 5 (size of advance run in KRL) + + // EKI socket read/write + int eki_read_state_timeout_ = 100; // [s]; settable by parameter (default = 5) + boost::asio::io_service ios_; + std::unique_ptr deadline_; + boost::asio::ip::udp::endpoint eki_server_endpoint_; + std::unique_ptr eki_server_socket_; + void eki_check_read_state_deadline(); + static void eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length); + bool eki_read_state(std::vector &joint_position, std::vector &joint_velocity, + std::vector &joint_effort, int &cmd_buff_len); + bool eki_write_command(const std::vector &joint_position); + }; + +} // namespace kuka_eki_hw_interface + +#endif // ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ diff --git a/kuka_eki_hw_interface/krl/README.md b/kuka_eki_hw_interface/krl/README.md index 5b5e7b54b..27cc8be31 100644 --- a/kuka_eki_hw_interface/krl/README.md +++ b/kuka_eki_hw_interface/krl/README.md @@ -42,7 +42,7 @@ We recommend that you copy the configuration files, edit the copies for your nee In order to successfully launch the **kuka_eki_hw_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application): ```xml - + ``` Make sure that the line is added before the `kuka_eki_hw_interface` itself is loaded. diff --git a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat index f47f888f3..644076067 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat @@ -1,4 +1,7 @@ &ACCESS RVO defdat kuka_eki_hw_interface ext bas(bas_command :in,real :in ) -enddat \ No newline at end of file + decl int io_pins[2] + decl int io_types[2] + decl int io_cmd[2] +enddat diff --git a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src index 5a4edf7b9..6a41a1abb 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src @@ -1,16 +1,17 @@ -&ACCESS RVO +&ACCESS RVP def kuka_eki_hw_interface() ; Software License Agreement (BSD License) ; ; Copyright (c) 2018, 3M + ; Modifications copyright (c) 2021 Gergely Sóti ; All rights reserved. ; ; Redistribution and use in source and binary forms, with or without ; modification, are permitted provided that the following conditions are met: ; ; * Redistributions of source code must retain the above copyright - ; notice, this list of conditions and the following disclaimer. + ; notice, this list of conditions and the following disclQaimer. ; * 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. @@ -30,12 +31,15 @@ def kuka_eki_hw_interface() ; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE ; POSSIBILITY OF SUCH DAMAGE. - ; Author: Brett Hemes (3M) - + ; This software is derived work of Brett Hemes (3M) + ; Adaptation by Gergely Sóti, H-KA iRAS Prof. Hein, 2021; gergely.soti@h-ka.de ; Declarations decl axis joint_pos_tgt decl int elements_read + decl int io_elements_read + decl int io_elements_set + decl int index ; INI bas(#initmov, 0) ; Basic initializasion of axes @@ -60,17 +64,35 @@ def kuka_eki_hw_interface() ; 15: Calls eki_hw_iface_reset() on falling edge of $flag[1] ; 16: Calls eki_hw_iface_send() on rising edge of $timer_flag[1] eki_hw_iface_init() + eki_io_iface_init() ; BCO (Block COincidence) run to current position ; (requied for below loop continue before first incoming command) joint_pos_tgt = $axis_act_meas + ; additional comment ptp joint_pos_tgt + for index = 1 to 2 + io_pins[index] = index + io_types[index] = 1 + io_cmd[index] = -1 + endfor + + eki_io_iface_send() + ; Loop forever $advance = 5 loop + ; TODO: check return values elements_read = eki_hw_iface_get(joint_pos_tgt) ; Get new command from buffer if present - ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position + if elements_read == 1 then + ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position + endif + io_elements_read = eki_io_iface_get() + if io_elements_read == 1 then + set_io_values() + eki_io_iface_send() + endif endloop ; Note: EKI channels delete on reset or deselect of this program @@ -98,7 +120,64 @@ def eki_hw_iface_init() eki_ret = eki_open("EkiHwInterface") end +def eki_io_iface_init() + decl eki_status eki_ret + + ; Setup interrupts + ; Interrupt 15 - Connection cleanup on client disconnect + ;global interrupt decl 17 when $flag[2]==false do eki_io_iface_reset() + ;interrupt on 17 + ; Interrupt 16 - Timer interrupt for periodic state transmission + ;global interrupt decl 18 when $timer_flag[2]==true do eki_io_iface_send() + ;interrupt on 18 + ;wait sec 0.012 ; Wait for next interpolation cycle + ;$timer[2] = -200 ; Time in [ms] before first interrupt call + ;$timer_stop[2] = false ; Start timer 1 + ; Create and open EKI interface + eki_ret = eki_init("EkiIOInterface") + eki_ret = eki_open("EkiIOInterface") +end + +def eki_io_iface_send() + decl eki_status eki_ret + decl int index + if $flag[2] then + index = 1 + eki_ret = eki_setint("EkiIOInterface", "IOState/IO1/@Type", io_types[index]) + eki_ret = eki_setint("EkiIOInterface", "IOState/IO1/@Pin", io_pins[index]) + switch io_types[index] + case 1 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", $in[io_pins[index]]) + case 2 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", $out[io_pins[index]]) + default + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", False) + endswitch + + index = 2 + eki_ret = eki_setint("EkiIOInterface", "IOState/IO2/@Type", io_types[index]) + eki_ret = eki_setint("EkiIOInterface", "IOState/IO2/@Pin", io_pins[index]) + switch io_types[index] + case 1 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", $in[io_pins[index]]) + case 2 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", $out[io_pins[index]]) + default + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", False) + endswitch + eki_ret = eki_checkbuffer("EkiIOInterface", "IOCommand/IO1/@Mode") + eki_ret = eki_setint("EkiIOInterface", "IOState/IOCommand/@Size", eki_ret.buff) + ; Send xml structure + if $flag[2] then ; Make sure connection hasn't died while updating xml structure + eki_ret = eki_send("EkiIOInterface", "IOState") + endif + endif + + + ; Set timer for next interrupt [ms] + $timer[2] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission +end def eki_hw_iface_send() decl eki_status eki_ret @@ -145,12 +224,13 @@ end deffct int eki_hw_iface_available() decl eki_status eki_ret - - if not $flag[1] then + continue + if not $flag[2] then return 0 endif eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") + $timer[2] = -10 return eki_ret.buff endfct @@ -168,8 +248,9 @@ deffct int eki_hw_iface_get(joint_pos_cmd :out) endif eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") + if eki_ret.buff <= 0 then - return 0 + return 0 endif eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A1", joint_pos_cmd.a1) @@ -181,12 +262,95 @@ deffct int eki_hw_iface_get(joint_pos_cmd :out) return 1 endfct +; eki_io_iface_get +; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Returns number of elements read. +deffct int eki_io_iface_get() + decl eki_status eki_ret + decl int index + decl int io_mode + decl int value + + if not $flag[2] then + return 0 + endif + + eki_ret = eki_checkbuffer("EkiIOInterface", "IOCommand/IO1/@Mode") + + if eki_ret.buff <= 0 then + return 0 + endif + + io_mode = 0 + index = 1 + value = -1 + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Mode", io_mode) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Pin", io_pins[index]) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Value", value) + switch io_mode + case 1 + io_types[index] = 1 + io_cmd[index] = -1 + case 2 + io_types[index] = 2 + io_cmd[index] = value + case 3 + io_types[index] = 2 + io_cmd[index] = -1 + default + io_types[index] = 0 + io_cmd[index] = -1 + endswitch + index = 2 + value = -1 + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Mode", io_mode) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Pin", io_pins[index]) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Value", value) + switch io_mode + case 1 + io_types[index] = 1 + io_cmd[index] = -1 + case 2 + io_types[index] = 2 + io_cmd[index] = value + case 3 + io_types[index] = 2 + io_cmd[index] = -1 + default + io_types[index] = 0 + io_cmd[index] = -1 + endswitch + + + return 1 +endfct +; set_io_values +; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Returns number of elements read. +def set_io_values() + decl int index + for index = 1 to 2 + if (io_cmd[index] <> -1) then + REPEAT + $out[io_pins[index]] = (io_cmd[index] <> 0) + UNTIL $out[io_pins[index]] == (io_cmd[index] <> 0) + endif + endfor +end def eki_hw_iface_reset() decl eki_status eki_ret - + eki_ret = eki_clear("EkiHwInterface") eki_ret = eki_init("EkiHwInterface") eki_ret = eki_open("EkiHwInterface") end + +def eki_io_iface_reset() + decl eki_status eki_ret + + eki_ret = eki_clear("EkiIOInterface") + eki_ret = eki_init("EkiIOInterface") + eki_ret = eki_open("EkiIOInterface") +end diff --git a/kuka_eki_hw_interface/launch/controller_spawner.launch b/kuka_eki_hw_interface/launch/controller_spawner.launch deleted file mode 100644 index bd10aabdc..000000000 --- a/kuka_eki_hw_interface/launch/controller_spawner.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - diff --git a/kuka_eki_hw_interface/launch/robot_interface_streaming.launch b/kuka_eki_hw_interface/launch/robot_interface_streaming.launch deleted file mode 100644 index b857c32f9..000000000 --- a/kuka_eki_hw_interface/launch/robot_interface_streaming.launch +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - eki: - robot_address: "$(arg robot_ip)" - robot_port: "$(arg port)" - - - - - $(arg js_ctrlr): - type: joint_state_controller/JointStateController - publish_rate: 50 - - # NOTE: these joint names MUST correspond to those specified in the - # 'controller_joint_names' parameter (typically loaded by the - # calling launch file) - $(arg pos_ctrlr_name): - type: "position_controllers/JointTrajectoryController" - joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 - - state_publish_rate: 50 # Defaults to 50 - action_monitor_rate: 20 # Defaults to 20 - - - - - - - - - - - - - diff --git a/kuka_eki_hw_interface/package.xml b/kuka_eki_hw_interface/package.xml index 915074672..bf3a841c6 100644 --- a/kuka_eki_hw_interface/package.xml +++ b/kuka_eki_hw_interface/package.xml @@ -1,24 +1,26 @@ - - kuka_eki_hw_interface - 0.0.1 - A ROS-Control hardware interface for use with KUKA EKI - Brett Hemes (3M) - Brett Hemes (3M) - G.A. vd. Hoorn (TU Delft Robotics Institute) - BSD + + + kuka_eki_hw_interface + 0.0.1 + ros2_control hardware interface through EKI + Gergely Sóti (H-KA) + Brett Hemes (3M) + Gergely Sóti (H-KA) + Apache 2.0 - http://wiki.ros.org/kuka_eki_hw_interface - https://github.com/ros-industrial/kuka_experimental/issues - https://github.com/ros-industrial/kuka_experimental + ament_cmake - catkin + ament_lint_auto + ament_lint_common - angles - cmake_modules + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + angles - controller_manager - hardware_interface - joint_limits_interface - roscpp + + ament_cmake + diff --git a/kuka_eki_hw_interface/ros2_control_kuka_eki.xml b/kuka_eki_hw_interface/ros2_control_kuka_eki.xml new file mode 100644 index 000000000..0e50359c0 --- /dev/null +++ b/kuka_eki_hw_interface/ros2_control_kuka_eki.xml @@ -0,0 +1,9 @@ + + + + Minimal hardware interface implementation for use with KUKA EKI. + + + \ No newline at end of file diff --git a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp index 9a59a03f1..05d00c0d8 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -1,305 +1,323 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 3M - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder, nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// Author: Brett Hemes (3M) - - -#include -#include -#include - -#include - -#include - -#include - +// Copyright 2020 ros2_control Development Team +// Modifications copyright (c) 2021 Gergely Sóti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "kuka_eki_hw_interface/kuka_eki_hw_interface.h" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" namespace kuka_eki_hw_interface { -KukaEkiHardwareInterface::KukaEkiHardwareInterface() : joint_position_(n_dof_, 0.0), joint_velocity_(n_dof_, 0.0), - joint_effort_(n_dof_, 0.0), joint_position_command_(n_dof_, 0.0), joint_names_(n_dof_), deadline_(ios_), - eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) -{ - -} - - -KukaEkiHardwareInterface::~KukaEkiHardwareInterface() {} - - -void KukaEkiHardwareInterface::eki_check_read_state_deadline() -{ - // Check if deadline has already passed - if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) - { - eki_server_socket_.cancel(); - deadline_.expires_at(boost::posix_time::pos_infin); - } - - // Sleep until deadline exceeded - deadline_.async_wait(boost::bind(&KukaEkiHardwareInterface::eki_check_read_state_deadline, this)); -} - - -void KukaEkiHardwareInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length, + KukaEkiHardwareInterface::~KukaEkiHardwareInterface() + { + // If the controller manager is shutdown via Ctrl + C the on_deactivate methods won't be called. + // We therefore need to make sure to actually deactivate the communication + on_deactivate(rclcpp_lifecycle::State()); + } + + hardware_interface::CallbackReturn KukaEkiHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) + { + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + + info_ = info; + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // RRBotSystemPositionOnly has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' has %ld command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' has %ld state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + } + +// status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::CallbackReturn::SUCCESS; + } + + std::vector KukaEkiHardwareInterface::export_state_interfaces() + { + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + return state_interfaces; + } + + std::vector KukaEkiHardwareInterface::export_command_interfaces() + { + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; + } + + void KukaEkiHardwareInterface::eki_check_read_state_deadline() + { + // Check if deadline has already passed + if (deadline_->expires_at() <= boost::asio::deadline_timer::traits_type::now()) + { + eki_server_socket_->cancel(); + deadline_->expires_at(boost::posix_time::pos_infin); + } + + // Sleep until deadline exceeded + deadline_->async_wait(boost::bind(&KukaEkiHardwareInterface::eki_check_read_state_deadline, this)); + } + + void KukaEkiHardwareInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length, boost::system::error_code* out_ec, size_t* out_length) -{ - *out_ec = ec; - *out_length = length; -} - - -bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_position, - std::vector &joint_velocity, - std::vector &joint_effort, - int &cmd_buff_len) -{ - static boost::array in_buffer; - - // Read socket buffer (with timeout) - // Based off of Boost documentation example: doc/html/boost_asio/example/timeouts/blocking_udp_client.cpp - deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline - boost::system::error_code ec = boost::asio::error::would_block; - size_t len = 0; - eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), - boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len)); - do - ios_.run_one(); - while (ec == boost::asio::error::would_block); - if (ec) - return false; - - // Update joint positions from XML packet (if received) - if (len == 0) - return false; - - // Parse XML - TiXmlDocument xml_in; - in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string) - xml_in.Parse(in_buffer.data()); - TiXmlElement* robot_state = xml_in.FirstChildElement("RobotState"); - if (!robot_state) - return false; - TiXmlElement* pos = robot_state->FirstChildElement("Pos"); - TiXmlElement* vel = robot_state->FirstChildElement("Vel"); - TiXmlElement* eff = robot_state->FirstChildElement("Eff"); - TiXmlElement* robot_command = robot_state->FirstChildElement("RobotCommand"); - if (!pos || !vel || !eff || !robot_command) - return false; - - // Extract axis positions - double joint_pos; // [deg] - double joint_vel; // [%max] - double joint_eff; // [Nm] - char axis_name[] = "A1"; - for (int i = 0; i < n_dof_; ++i) - { - pos->Attribute(axis_name, &joint_pos); - joint_position[i] = angles::from_degrees(joint_pos); // convert deg to rad - vel->Attribute(axis_name, &joint_vel); - joint_velocity[i] = joint_vel; - eff->Attribute(axis_name, &joint_eff); - joint_effort[i] = joint_eff; - axis_name[1]++; - } - - // Extract number of command elements buffered on robot - robot_command->Attribute("Size", &cmd_buff_len); - - return true; -} - - -bool KukaEkiHardwareInterface::eki_write_command(const std::vector &joint_position_command) -{ - TiXmlDocument xml_out; - TiXmlElement* robot_command = new TiXmlElement("RobotCommand"); - TiXmlElement* pos = new TiXmlElement("Pos"); - TiXmlText* empty_text = new TiXmlText(""); - robot_command->LinkEndChild(pos); - pos->LinkEndChild(empty_text); // force format (vs ) - char axis_name[] = "A1"; - for (int i = 0; i < n_dof_; ++i) - { - pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str()); - axis_name[1]++; - } - xml_out.LinkEndChild(robot_command); - - TiXmlPrinter xml_printer; - xml_printer.SetStreamPrinting(); // no linebreaks - xml_out.Accept(&xml_printer); - - size_t len = eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), - eki_server_endpoint_); - - return true; -} - - -void KukaEkiHardwareInterface::init() -{ - // Get controller joint names from parameter server - if (!nh_.getParam("controller_joint_names", joint_names_)) - { - ROS_ERROR("Cannot find required parameter 'controller_joint_names' on the parameter server."); - throw std::runtime_error("Cannot find required parameter 'controller_joint_names' on the parameter server."); - } - - // Get EKI parameters from parameter server - const std::string param_addr = "eki/robot_address"; - const std::string param_port = "eki/robot_port"; - const std::string param_socket_timeout = "eki/socket_timeout"; - const std::string param_max_cmd_buf_len = "eki/max_cmd_buf_len"; - - if (nh_.getParam(param_addr, eki_server_address_) && - nh_.getParam(param_port, eki_server_port_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface on: " - << eki_server_address_ << ", " << eki_server_port_); - } - else - { - std::string msg = "Failed to get EKI address/port from parameter server (looking for '" + param_addr + - "', '" + param_port + "')"; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } - - if (nh_.getParam(param_socket_timeout, eki_read_state_timeout_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface socket timeout to " - << eki_read_state_timeout_ << " seconds"); - } - else - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI socket timeout from parameter server (looking " - "for '" + param_socket_timeout + "'), defaulting to " + - std::to_string(eki_read_state_timeout_) + " seconds"); - } - - if (nh_.getParam(param_max_cmd_buf_len, eki_max_cmd_buff_len_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface maximum command buffer " - "length to " << eki_max_cmd_buff_len_); - } - else - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI hardware interface maximum command buffer length " - "from parameter server (looking for '" + param_max_cmd_buf_len + "'), defaulting to " + - std::to_string(eki_max_cmd_buff_len_)); - } - - // Create ros_control interfaces (joint state and position joint for all dof's) - for (std::size_t i = 0; i < n_dof_; ++i) - { - // Joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); - - // Joint position control interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); - } - - // Register interfaces - registerInterface(&joint_state_interface_); - registerInterface(&position_joint_interface_); - - ROS_INFO_STREAM_NAMED("kukw_eki_hw_interface", "Loaded Kuka EKI hardware interface"); -} - - -void KukaEkiHardwareInterface::start() -{ - ROS_INFO_NAMED("kuka_eki_hw_interface", "Starting Kuka EKI hardware interface..."); - - // Start client - ROS_INFO_NAMED("kuka_eki_hw_interface", "... connecting to robot's EKI server..."); - boost::asio::ip::udp::resolver resolver(ios_); - eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); - boost::array ini_buf = { 0 }; - eki_server_socket_.send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); // initiate contact to start server - - // Start persistent actor to check for eki_read_state timeouts - deadline_.expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf) - eki_check_read_state_deadline(); - - // Initialize joint_position_command_ from initial robot state (avoid bad (null) commands before controllers come up) - if (!eki_read_state(joint_position_, joint_velocity_, joint_effort_, eki_cmd_buff_len_)) - { - std::string msg = "Failed to read from robot EKI server within alloted time of " - + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " - "on the robot controller and all configurations are correct."; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } - joint_position_command_ = joint_position_; - - ROS_INFO_NAMED("kuka_eki_hw_interface", "... done. EKI hardware interface started!"); -} - - -void KukaEkiHardwareInterface::read(const ros::Time &time, const ros::Duration &period) -{ - if (!eki_read_state(joint_position_, joint_velocity_, joint_effort_, eki_cmd_buff_len_)) - { - std::string msg = "Failed to read from robot EKI server within alloted time of " - + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " - "on the robot controller and all configurations are correct."; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } -} - - -void KukaEkiHardwareInterface::write(const ros::Time &time, const ros::Duration &period) -{ - // only write if max will not be exceeded - if (eki_cmd_buff_len_ < eki_max_cmd_buff_len_) - eki_write_command(joint_position_command_); - - // underflow/overflow checking - // NOTE: this is commented as it results in a lot of logging output and the use of ROS_* - // logging macros breaks incurs (quite) some overhead. Uncomment and rebuild this - // if you'd like to use this anyway. - //if (eki_cmd_buff_len_ >= eki_max_cmd_buff_len_) - // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer overflow (curent size " << eki_cmd_buff_len_ - // << " greater than or equal max allowed " << eki_max_cmd_buff_len_ << ")"); - //else if (eki_cmd_buff_len_ == 0) - // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer empty"); -} - -} // namespace kuka_eki_hw_interface + { + *out_ec = ec; + *out_length = length; + } + + bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_position, + std::vector &joint_velocity, + std::vector &joint_effort, + int &cmd_buff_len) + { + static boost::array in_buffer; + // Read socket buffer (with timeout) + // Based off of Boost documentation example: doc/html/boost_asio/example/timeouts/blocking_udp_client.cpp + deadline_->expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline + boost::system::error_code ec = boost::asio::error::would_block; + size_t len = 0; + + eki_server_socket_->async_receive(boost::asio::buffer(in_buffer), + boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len)); + do + ios_.run_one(); + while (ec == boost::asio::error::would_block); + + if (ec) + { + RCLCPP_WARN(rclcpp::get_logger("KukaEkiHardwareInterface"), " communication error code: %s", ec.message().c_str()); + return false; + } + + // Update joint positions from XML packet (if received) + if (len == 0) + { + RCLCPP_WARN(rclcpp::get_logger("KukaEkiHardwareInterface"), " packet of len 0 received"); + return false; + } + + // Parse XML + TiXmlDocument xml_in; + in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string) + xml_in.Parse(in_buffer.data()); + TiXmlElement* robot_state = xml_in.FirstChildElement("RobotState"); + if (!robot_state) + return false; + TiXmlElement* pos = robot_state->FirstChildElement("Pos"); + TiXmlElement* vel = robot_state->FirstChildElement("Vel"); + TiXmlElement* eff = robot_state->FirstChildElement("Eff"); + TiXmlElement* robot_command = robot_state->FirstChildElement("RobotCommand"); + if (!pos || !vel || !eff || !robot_command) + { + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "parsing failed, one of pos, vel eff or RobotCommand missing"); + return false; + } + + // Extract axis positions + double joint_pos; // [deg] + double joint_vel; // [%max] + double joint_eff; // [Nm] + char axis_name[] = "A1"; + for (long unsigned int i = 0; i < hw_states_.size(); ++i) + { + pos->Attribute(axis_name, &joint_pos); + joint_position[i] = angles::from_degrees(joint_pos); // convert deg to rad + vel->Attribute(axis_name, &joint_vel); + joint_velocity[i] = joint_vel; + eff->Attribute(axis_name, &joint_eff); + joint_effort[i] = joint_eff; + axis_name[1]++; + } + + // Extract number of command elements buffered on robot + robot_command->Attribute("Size", &cmd_buff_len); + return true; + } + + bool KukaEkiHardwareInterface::eki_write_command(const std::vector &joint_position_command) + { + TiXmlDocument xml_out; + TiXmlElement* robot_command = new TiXmlElement("RobotCommand"); + TiXmlElement* pos = new TiXmlElement("Pos"); + TiXmlText* empty_text = new TiXmlText(""); + robot_command->LinkEndChild(pos); + pos->LinkEndChild(empty_text); // force format (vs ) + char axis_name[] = "A1"; + for (long unsigned int i = 0; i < hw_states_.size(); ++i) + { + pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str()); + axis_name[1]++; + } + xml_out.LinkEndChild(robot_command); + TiXmlPrinter xml_printer; + xml_printer.SetStreamPrinting(); // no linebreaks + xml_out.Accept(&xml_printer); + + eki_server_socket_->send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), + eki_server_endpoint_); + + return true; + } + + hardware_interface::CallbackReturn KukaEkiHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) + { + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "Starting ... please wait..."); + + eki_server_address_ = info_.hardware_parameters["robot_ip"]; + eki_server_port_ = info_.hardware_parameters["eki_robot_port"]; + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "using IP: %s", eki_server_address_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "using port: %s", eki_server_port_.c_str()); + + if (eki_server_address_.empty() || eki_server_port_.empty()) + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaEkiHardwareInterface"), "robot_ip or eki_robot_port cannot be empty"); + return hardware_interface::CallbackReturn::ERROR; + } + + deadline_.reset(new boost::asio::deadline_timer(ios_)); + eki_server_socket_.reset(new boost::asio::ip::udp::socket(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0))); + + boost::asio::ip::udp::resolver resolver(ios_); + eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); + + boost::array ini_buf = { 0 }; + eki_server_socket_->send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); // initiate contact to start server + + // Start persistent actor to check for eki_read_state timeouts + deadline_->expires_at(boost::posix_time::pos_infin); // do nothing until a read is invoked (deadline_ = +inf) + eki_check_read_state_deadline(); + + std::vector joint_position; + std::vector joint_velocity; + std::vector joint_effort; + joint_position.resize(hw_states_.size()); + joint_velocity.resize(hw_states_.size()); + joint_effort.resize(hw_states_.size()); + if (!eki_read_state(joint_position, joint_velocity, joint_effort, eki_cmd_buff_len_)) + { + std::string msg = "Failed to read from robot EKI server within alloted time of " + + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " + "on the robot controller and all configurations are correct."; + RCLCPP_ERROR( + rclcpp::get_logger("KukaEkiHardwareInterface"), msg.c_str()); + throw std::runtime_error(msg); + } + hw_commands_ = joint_position; + hw_states_ = joint_position; + + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "System Successfully started!"); + + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn KukaEkiHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& previous_state) + { + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "Stopping ...please wait..."); + + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "System successfully stopped!"); + + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::return_type KukaEkiHardwareInterface::read(const rclcpp::Time& time, + const rclcpp::Duration& period) + { + std::vector joint_position; + std::vector joint_velocity; + std::vector joint_effort; + joint_position.resize(hw_states_.size()); + joint_velocity.resize(hw_states_.size()); + joint_effort.resize(hw_states_.size()); + if (!eki_read_state(joint_position, joint_velocity, joint_effort, eki_cmd_buff_len_)) + { + std::string msg = "Failed to read from robot EKI server within alloted time of " + + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " + "on the robot controller and all configurations are correct."; + RCLCPP_ERROR( + rclcpp::get_logger("KukaEkiHardwareInterface"), msg.c_str()); + throw std::runtime_error(msg); + } + hw_states_ = joint_position; + + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type KukaEkiHardwareInterface::write(const rclcpp::Time& time, + const rclcpp::Duration& period) + { + if (eki_cmd_buff_len_ < eki_max_cmd_buff_len_) + eki_write_command(hw_commands_); + return hardware_interface::return_type::OK; + } + +} // namespace kuka_eki_hw_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + kuka_eki_hw_interface::KukaEkiHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp deleted file mode 100644 index b3f944dd9..000000000 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 3M - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder, nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// Author: Brett Hemes (3M) - -#include - -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "kuka_eki_hw_interface"); - - ros::AsyncSpinner spinner(2); - spinner.start(); - - ros::NodeHandle nh; - - kuka_eki_hw_interface::KukaEkiHardwareInterface hardware_interface; - hardware_interface.init(); - - // Set up timers - ros::Time timestamp; - ros::Duration period; - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - - controller_manager::ControllerManager controller_manager(&hardware_interface, nh); - - hardware_interface.start(); - - // Get current time and elapsed time since last read - timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; - - while (ros::ok()) - { - // Receive current state from robot - hardware_interface.read(timestamp, period); - - // Get current time and elapsed time since last read - timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; - - // Update the controllers - controller_manager.update(timestamp, period); - - // Send new setpoint to robot - hardware_interface.write(timestamp, period); - } - - spinner.stop(); - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Shutting down."); - - return 0; -} diff --git a/kuka_eki_hw_interface/test/test_hardware_interface.launch b/kuka_eki_hw_interface/test/test_hardware_interface.launch deleted file mode 100644 index dbbbf76ac..000000000 --- a/kuka_eki_hw_interface/test/test_hardware_interface.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/kuka_eki_hw_interface/test/test_params.yaml b/kuka_eki_hw_interface/test/test_params.yaml deleted file mode 100644 index 31a506e9d..000000000 --- a/kuka_eki_hw_interface/test/test_params.yaml +++ /dev/null @@ -1,4 +0,0 @@ -eki: - robot_address: "address.of.robot.controller" - robot_port: "54600" - socket_timeout: 5 diff --git a/kuka_eki_io_interface/CMakeLists.txt b/kuka_eki_io_interface/CMakeLists.txt new file mode 100644 index 000000000..9cc6c9ce4 --- /dev/null +++ b/kuka_eki_io_interface/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) +project(kuka_eki_io_interface) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(tinyxml_vendor REQUIRED) +find_package(TinyXML REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_library( + ${PROJECT_NAME} + SHARED + src/kuka_eki_io_interface.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) +ament_target_dependencies( + ${PROJECT_NAME} + rclcpp + rclcpp_lifecycle + TinyXML +) + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle +) + + +### ros2 control hardware plugin +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) + +add_executable(eki_io_node src/kuka_eki_io_interface_node.cpp src/kuka_eki_io_interface.cpp) +target_include_directories( + eki_io_node + PRIVATE + include) +ament_target_dependencies(eki_io_node ${${PROJECT_NAME}_EXPORTED_TARGETS} rclcpp std_msgs rclcpp_lifecycle TinyXML) +install(TARGETS + eki_io_node + DESTINATION lib/${PROJECT_NAME}) + + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h b/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h new file mode 100644 index 000000000..3a772bbbd --- /dev/null +++ b/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h @@ -0,0 +1,50 @@ +// Copyright (c) 2021 Gergely Sóti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef KUKA_EKI_IO_INTERFACE +#define KUKA_EKI_IO_INTERFACE + +#include +#include + +#include + +namespace kuka_eki_io_interface +{ + class KukaEkiIOInterface + { + private: + std::string eki_server_address_; + std::string eki_server_port_; + int eki_cmd_buff_len_; + int eki_max_cmd_buff_len_ = 5; + int n_io_; + + int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) + static void eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length); + boost::asio::io_service ios_; + boost::asio::deadline_timer deadline_; + boost::asio::ip::udp::endpoint eki_server_endpoint_; + boost::asio::ip::udp::socket eki_server_socket_; + void eki_check_read_state_deadline(); + + public: + KukaEkiIOInterface(const std::string& eki_server_address, const std::string& eki_server_port, int n_io); + ~KukaEkiIOInterface(){}; + bool eki_read_state(std::vector &io_states, std::vector &io_pins, std::vector &io_types, int &cmd_buff_len); + bool eki_write_command(const std::vector &io_pins, const std::vector &io_modes, const std::vector &target_ios); + }; +} +#endif \ No newline at end of file diff --git a/kuka_eki_io_interface/launch/eki_io_interface.launch.py b/kuka_eki_io_interface/launch/eki_io_interface.launch.py new file mode 100644 index 000000000..4b61223e0 --- /dev/null +++ b/kuka_eki_io_interface/launch/eki_io_interface.launch.py @@ -0,0 +1,45 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append(DeclareLaunchArgument( + "robot_ip", + default_value="10.181.116.41", + description="IP address by which the robot can be reached." + )) + + declared_arguments.append(DeclareLaunchArgument( + "eki_io_port", + default_value="54601", + description="Port by which the robot can be reached." + )) + + declared_arguments.append(DeclareLaunchArgument( + "n_io", + default_value="2", + description="Port by which the robot can be reached." + )) + + robot_ip = LaunchConfiguration("robot_ip") + eki_io_port = LaunchConfiguration("eki_io_port") + n_io = LaunchConfiguration("n_io") + + eki_io_node = Node( + package="kuka_eki_io_interface", + executable="eki_io_node", + parameters=[ + {"robot_ip": robot_ip, + "eki_io_port": eki_io_port, + "n_io": n_io} + ], + emulate_tty=True, + output='screen' + ) + return LaunchDescription(declared_arguments + [ + eki_io_node + ]) diff --git a/kuka_eki_io_interface/package.xml b/kuka_eki_io_interface/package.xml new file mode 100644 index 000000000..040126b5a --- /dev/null +++ b/kuka_eki_io_interface/package.xml @@ -0,0 +1,24 @@ + + + + kuka_eki_io_interface + 0.0.1 + ros2_control io interface through EKI + Gergely Sóti (H-KA) + Gergely Sóti (H-KA) + Apache 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_lifecycle + rclcpp_components + std_msgs + + + ament_cmake + + diff --git a/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp b/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp new file mode 100644 index 000000000..ffdcbeace --- /dev/null +++ b/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2021 Gergely Sóti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +namespace kuka_eki_io_interface +{ + KukaEkiIOInterface::KukaEkiIOInterface(const std::string& eki_server_address, const std::string& eki_server_port, int n_io) : deadline_(ios_), + eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) + { + eki_server_address_ = eki_server_address; + eki_server_port_ = eki_server_port; + n_io_ = n_io; + + std::cout << eki_server_address_ << std::endl; + std::cout << eki_server_port_ << std::endl; + std::cout << n_io_ << std::endl; + + boost::asio::ip::udp::resolver resolver(ios_); + eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); + + boost::array ini_buf = { 0 }; + eki_server_socket_.send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); + + deadline_.expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf) + eki_check_read_state_deadline(); + +// std::vector io_states; +// std::vector io_pins; +// std::vector io_types; +// int buff_len; +// if (!eki_read_state(io_states, io_pins, io_types, buff_len)) +// { +// std::string msg = "Failed to read from robot EKI server within alloted time of " +// + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " +// "on the robot controller and all configurations are correct."; +// std::cout << msg << std::endl; +// throw std::runtime_error(msg); +// } + } +// KukaEkiIOInterface::~KukaEkiIOInterface() {} + + void KukaEkiIOInterface::eki_check_read_state_deadline() + { + // Check if deadline has already passed + if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) + { + eki_server_socket_.cancel(); + deadline_.expires_at(boost::posix_time::pos_infin); + } + // Sleep until deadline exceeded + deadline_.async_wait(boost::bind(&KukaEkiIOInterface::eki_check_read_state_deadline, this)); + } + + bool KukaEkiIOInterface::eki_read_state(std::vector &io_states, std::vector &io_pins, std::vector &io_types, int &cmd_buff_len) + { + io_states.resize(n_io_); + io_pins.resize(n_io_); + io_types.resize(n_io_); + static boost::array in_buffer; + deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline + boost::system::error_code ec = boost::asio::error::would_block; + size_t len = 0; + eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), + boost::bind(&KukaEkiIOInterface::eki_handle_receive, _1, _2, &ec, &len)); + + do { + ios_.run_one(); + } + while (ec == boost::asio::error::would_block); + if (ec) + { + std::cout << ec << std::endl; + return false; + } + if (len == 0) + { + std::cout << "2" << std::endl; + return false; + } + + TiXmlDocument xml_in; + in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string) + xml_in.Parse(in_buffer.data()); + TiXmlElement* robot_state = xml_in.FirstChildElement("IOState"); + if (!robot_state) + { + std::cout << "3" << std::endl; + return false; + } + xml_in.Print(); + char io_name[] = "IO1"; + for (int i = 0; i < n_io_; ++i) + { + TiXmlElement* state = robot_state->FirstChildElement(io_name); + if (!state) + { + std::cout << "4" << std::endl; + return false; + } + int io_state; // [Nm] + state->Attribute("State", &io_state); + int io_pin; + state->Attribute("Pin", &io_pin); + int io_type; + state->Attribute("Type", &io_type); + io_states[i] = io_state; + io_pins[i] = io_pin; + io_types[i] = io_type; + io_name[2]++; + } + TiXmlElement* robot_command = robot_state->FirstChildElement("IOCommand"); + robot_command->Attribute("Size", &cmd_buff_len); + return true; + } + + bool KukaEkiIOInterface::eki_write_command(const std::vector &io_pins, const std::vector &io_modes, const std::vector &target_ios) + { + // TODO: assert vectors' lengths + // TODO: extend vector if length < n_io_ + TiXmlDocument xml_out; + TiXmlElement* io_command = new TiXmlElement("IOCommand"); + char io_name[] = "IO1"; + for (int i = 0; i < n_io_; i++) + { + TiXmlElement* io_element = new TiXmlElement(io_name); + TiXmlText* empty_text = new TiXmlText(""); + io_command->LinkEndChild(io_element); + io_element->LinkEndChild(empty_text); + + io_element->SetAttribute("Pin", io_pins[i]); + io_element->SetAttribute("Mode", io_modes[i]); + io_element->SetAttribute("Value", (int)target_ios[i]); + + io_name[2]++; + } + xml_out.LinkEndChild(io_command); + xml_out.Print(); + TiXmlPrinter xml_printer; + xml_printer.SetStreamPrinting(); // no linebreaks + xml_out.Accept(&xml_printer); + + eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), + eki_server_endpoint_); + return true; + } + + void KukaEkiIOInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length) + { + *out_ec = ec; + *out_length = length; + } +} \ No newline at end of file diff --git a/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp b/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp new file mode 100644 index 000000000..ef2a99d45 --- /dev/null +++ b/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2021 Gergely Sóti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "kuka_eki_io_interface/kuka_eki_io_interface.h" + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + std::shared_ptr node = rclcpp::Node::make_shared("eki_io_node"); + + rclcpp::Rate loop_rate(2); + node->declare_parameter("robot_ip", "0.0.0.0"); + node->declare_parameter("eki_io_port", 54601); + node->declare_parameter("n_io", 2); + + std::string robot_ip = node->get_parameter("robot_ip").as_string(); + int eki_io_port = node->get_parameter("eki_io_port").as_int(); + std::string eki_io_port_ = std::to_string(eki_io_port); + int n_io = node->get_parameter("n_io").as_int(); + kuka_eki_io_interface::KukaEkiIOInterface io_interface(robot_ip.c_str(), eki_io_port_.c_str(), n_io); +// + const std::vector io_pins_cmd{7, 8}; + const std::vector io_modes_cmd{2, 2}; + const std::vector target_ios_cmd{ false, true}; +// io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + + loop_rate.sleep(); + while (rclcpp::ok()) + { + rclcpp::spin_some(node); + io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + + rclcpp::spin_some(node); + std::vector io_states; + std::vector io_pins; + std::vector io_types; + int buff_len; + io_interface.eki_read_state(io_states, io_pins, io_types, buff_len); +// for(int i = 0; i < n_io; i++) +// { +// RCLCPP_INFO(node->get_logger(), "IO state: %d, %d, %d", io_pins[i], io_types[1], io_states[i]); +// } + loop_rate.sleep(); + } + for(int i = 0; i<10; i++) { + io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + } + + RCLCPP_INFO(rclcpp::get_logger("eki_io_node"), "Shutting down."); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/kuka_eki_simulator/CMakeLists.txt b/kuka_eki_simulator/CMakeLists.txt new file mode 100644 index 000000000..578c48143 --- /dev/null +++ b/kuka_eki_simulator/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.16) +project(kuka_eki_simulator LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(kuka_eki_simulator + SCRIPTS_DESTINATION lib/kuka_eki_simulator +) +ament_package() diff --git a/kuka_eki_simulator/README.md b/kuka_eki_simulator/README.md new file mode 100644 index 000000000..c59940ad0 --- /dev/null +++ b/kuka_eki_simulator/README.md @@ -0,0 +1,3 @@ +# KUKA Simulation + +This package contains components for simulation of KUKA robots using the KUKA.Eki. diff --git a/kuka_eki_simulator/kuka_eki_simulator/__init__.py b/kuka_eki_simulator/kuka_eki_simulator/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/kuka_eki_simulator/kuka_eki_simulator/kuka_eki_simulator.py b/kuka_eki_simulator/kuka_eki_simulator/kuka_eki_simulator.py new file mode 100755 index 000000000..7a8b5fd56 --- /dev/null +++ b/kuka_eki_simulator/kuka_eki_simulator/kuka_eki_simulator.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 + +import argparse +import sys +import socket +import numpy as np +import time +import xml.etree.ElementTree as ET +import errno +import rclpy +from std_msgs.msg import String + +max_vel = 1.0 * 100.0 + + +def create_eki_xml_rob(act_joint_pos): + q = act_joint_pos + qd = [0.0]*6 + eff = [0.0]*6 + + root = ET.Element('RobotState') + + ET.SubElement(root, 'Pos', {'A1': str(q[0]), 'A2': str(q[1]), 'A3': str(q[2]), + 'A4': str(q[3]), 'A5': str(q[4]), 'A6': str(q[5])}) + ET.SubElement(root, 'Vel', {'A1': str(qd[0]/max_vel), 'A2': str(qd[1]/max_vel), 'A3': str(qd[2]/max_vel), + 'A4': str(qd[3]/max_vel), 'A5': str(qd[4]/max_vel), 'A6': str(qd[5]/max_vel)}) + ET.SubElement(root, 'Eff', {'A1': str(eff[0]), 'A2': str(eff[1]), 'A3': str(eff[2]), + 'A4': str(eff[3]), 'A5': str(eff[4]), 'A6': str(eff[5])}) + ET.SubElement(root, 'RobotCommand', {'Size': str(1)}) + + return ET.tostring(root, short_empty_elements=False) + + +def parse_eki_xml_sen(data): + desired_joint = None + + try: + root = ET.fromstring(data) + if root: + if root.tag == 'RobotCommand': + pos = root.find('Pos').attrib + if pos: + desired_joint = np.array([pos['A1'], pos['A2'], pos['A3'], + pos['A4'], pos['A5'], pos['A6']], dtype=np.float64) + else: + print(f' RobotCommand not found in data {root}') + + except: + print(f' Cannot parse this data {data}') + pass + + return desired_joint + + +def main(args=None): + rclpy.init(args=args) + parser = argparse.ArgumentParser(description='KUKA RSI Simulation') + parser.add_argument('--eki_hw_iface_ip', default="127.0.0.1", help='The ip address of the EKI control interface (default=127.0.0.1)') + parser.add_argument('--eki_hw_iface_port', default=54600, help='The port of the EKI control interface (default=54600)') + parser.add_argument('--sen', default='ImFree', help='Type attribute in EKI XML doc. E.g. ') + # Only parse known arguments + args, _ = parser.parse_known_args() + host = args.eki_hw_iface_ip + port = int(args.eki_hw_iface_port) + sen_type = args.sen + + # Configuration + node_name = 'kuka_eki_simulation' + cycle_time = 0.004 + act_joint_pos = np.array([0, -90, 90, 0, 90, 0], dtype=np.float64) + cmd_joint_pos = act_joint_pos.copy() + des_joint_absolute = np.zeros(6) + timeout_count = 0 + max_timeout = 5 + + node = rclpy.create_node(node_name) + + node.get_logger().info(f"Started '{node_name}' node.") + + rsi_act_pub = node.create_publisher(String, '~/eki/state', 1) + rsi_cmd_pub = node.create_publisher(String, '~/eki/command', 1) + + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + node.get_logger().info(f"Successfully created socket on ip:port {host}:{port}.") + + except socket.error as e: + node.get_logger().fatal(f"Could not create socket.") + sys.exit() + + node.get_logger().info(f"Waiting for first connection.") + s.bind((host, port)) + + while rclpy.ok(): + # wait for connection (will be a single char) + s.settimeout(100000) + recv_msg, addr = s.recvfrom(1024) + node.get_logger().info(f"Connected to {addr}.") + + s.settimeout(1) + timeout_count = 0 + while rclpy.ok() and timeout_count < max_timeout: + time.sleep(0.001) # FIXME: make this a ros2 node + try: + str_data = create_eki_xml_rob(act_joint_pos) + msg = String() + msg.data = str(str_data) + rsi_act_pub.publish(msg) + s.sendto(str_data, addr) + + recv_msg, addr = s.recvfrom(1024) + msg = String() + msg.data = str(recv_msg) + rsi_cmd_pub.publish(msg) + des_joint_absolute = parse_eki_xml_sen(recv_msg) + if des_joint_absolute is not None: + act_joint_pos = des_joint_absolute + else: + continue + time.sleep(cycle_time / 2) + except socket.timeout: + node.get_logger().info(f"Socket timed out.") + timeout_count += 1 + except socket.error as e: + if e.errno != errno.EINTR: + raise + node.get_logger().info(f"Connected to {addr}.") + + node.get_logger().info(f"Shutting down '{node_name}' node.") + node.destroy_node() + rclpy.shutdown() + s.close() diff --git a/kuka_eki_simulator/package.xml b/kuka_eki_simulator/package.xml new file mode 100644 index 000000000..952c6b99d --- /dev/null +++ b/kuka_eki_simulator/package.xml @@ -0,0 +1,24 @@ + + + kuka_eki_simulator + 0.1.0 + Python node that implements a minimal EKI interface simulator. + Lars Tingelstad + Guillaume Walck + Dr. Denis Štogl + BSD + + http://wiki.ros.org/kuka_eki_simulator + https://github.com/ros-industrial/kuka_experimental/issues + https://github.com/ros-industrial/kuka_experimental + + ament_cmake + ament_cmake_python + + rclpy + std_msgs + + + ament_cmake + + diff --git a/kuka_eki_simulator/setup.cfg b/kuka_eki_simulator/setup.cfg new file mode 100644 index 000000000..10cb432d2 --- /dev/null +++ b/kuka_eki_simulator/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +console_scripts = + kuka_eki_simulator = kuka_eki_simulator.kuka_eki_simulator:main diff --git a/kuka_experimental-not-released.foxy.repos b/kuka_experimental-not-released.foxy.repos new file mode 100644 index 000000000..1b3910e7e --- /dev/null +++ b/kuka_experimental-not-released.foxy.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/kuka_experimental-not-released.rolling.repos b/kuka_experimental-not-released.rolling.repos new file mode 100644 index 000000000..1b3910e7e --- /dev/null +++ b/kuka_experimental-not-released.rolling.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/kuka_experimental.foxy.repos b/kuka_experimental.foxy.repos new file mode 100644 index 000000000..db30b7f9f --- /dev/null +++ b/kuka_experimental.foxy.repos @@ -0,0 +1,9 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: foxy + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: foxy diff --git a/kuka_experimental.rolling.repos b/kuka_experimental.rolling.repos new file mode 100644 index 000000000..2d720029d --- /dev/null +++ b/kuka_experimental.rolling.repos @@ -0,0 +1,9 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: master diff --git a/kuka_experimental/CMakeLists.txt b/kuka_experimental/CMakeLists.txt index 8761cb10d..e4f2730dc 100644 --- a/kuka_experimental/CMakeLists.txt +++ b/kuka_experimental/CMakeLists.txt @@ -1,4 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_experimental) -find_package(catkin REQUIRED) -catkin_metapackage() +cmake_minimum_required(VERSION 3.16) + +project(kuka_experimental LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/kuka_experimental/package.xml b/kuka_experimental/package.xml index c9c20c0ce..bea263cb2 100644 --- a/kuka_experimental/package.xml +++ b/kuka_experimental/package.xml @@ -1,17 +1,20 @@ - + + kuka_experimental 0.1.0 Experimental packages for KUKA manipulators within ROS-Industrial. Shaun Edwards G.A. vd. Hoorn (TU Delft Robotics Institute) BSD + Dr. Denis Štogl (Stogl Robotics Consulting) + BSD http://wiki.ros.org/kuka_experimental https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake kuka_kr10_support kuka_kr120_support @@ -22,11 +25,12 @@ kuka_kr6_support kuka_lbr_iiwa_support kuka_resources - kuka_eki_hw_interface + kuka_ros2_control_support + kuka_rsi_hw_interface kuka_rsi_simulator - + ament_cmake diff --git a/kuka_kr10_support/CMakeLists.txt b/kuka_kr10_support/CMakeLists.txt index 7e8da3a67..106e2ddcc 100644 --- a/kuka_kr10_support/CMakeLists.txt +++ b/kuka_kr10_support/CMakeLists.txt @@ -1,15 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) +project(kuka_kr10_support LANGUAGES CXX) -project(kuka_kr10_support) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) -catkin_package() -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test_kr10r1100sixx.xml) -endif() +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_package() diff --git a/kuka_kr10_support/config/joint_names_kr10r1100sixx.yaml b/kuka_kr10_support/config/joint_names_kr10r1100sixx.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr10_support/config/joint_names_kr10r1100sixx.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr10_support/launch/load_kr10r1100sixx.launch b/kuka_kr10_support/launch/load_kr10r1100sixx.launch deleted file mode 100644 index dae4ecd42..000000000 --- a/kuka_kr10_support/launch/load_kr10r1100sixx.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/kuka_kr10_support/launch/test_kr10r1100sixx.launch b/kuka_kr10_support/launch/test_kr10r1100sixx.launch deleted file mode 100644 index 2b5cf6e0c..000000000 --- a/kuka_kr10_support/launch/test_kr10r1100sixx.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr10_support/launch/test_kr10r1100sixx.launch.py b/kuka_kr10_support/launch/test_kr10r1100sixx.launch.py new file mode 100644 index 000000000..eb22ad142 --- /dev/null +++ b/kuka_kr10_support/launch/test_kr10r1100sixx.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr10_support", + "robot_description_file": "kr10r1100sixx.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr10_support/launch/test_kr10r1420.launch.py b/kuka_kr10_support/launch/test_kr10r1420.launch.py new file mode 100644 index 000000000..edf7a8e35 --- /dev/null +++ b/kuka_kr10_support/launch/test_kr10r1420.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr10_support", + "robot_description_file": "kr10r1420.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr10_support/launch/test_kr10r900_2.launch.py b/kuka_kr10_support/launch/test_kr10r900_2.launch.py new file mode 100644 index 000000000..7f24dcd3f --- /dev/null +++ b/kuka_kr10_support/launch/test_kr10r900_2.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr10_support", + "robot_description_file": "kr10r900_2.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/base_link.stl b/kuka_kr10_support/meshes/kr10r1420/collision/base_link.stl new file mode 100644 index 000000000..e193b69f8 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/base_link.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_1.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_1.stl new file mode 100644 index 000000000..0a175dcb7 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_1.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_2.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_2.stl new file mode 100644 index 000000000..6afe34ed9 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_2.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_3.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_3.stl new file mode 100644 index 000000000..1e3679fd0 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_3.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_4.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_4.stl new file mode 100644 index 000000000..96a1320da Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_4.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_5.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_5.stl new file mode 100644 index 000000000..c47466677 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_5.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/collision/link_6.stl b/kuka_kr10_support/meshes/kr10r1420/collision/link_6.stl new file mode 100644 index 000000000..6f5dab3be Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/collision/link_6.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/base_link.stl b/kuka_kr10_support/meshes/kr10r1420/visual/base_link.stl new file mode 100644 index 000000000..c76dd434c Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/base_link.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_1.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_1.stl new file mode 100644 index 000000000..6d4c89e5f Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_1.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_2.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_2.stl new file mode 100644 index 000000000..4d0ee64f5 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_2.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_3.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_3.stl new file mode 100644 index 000000000..0aa4b6762 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_3.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_4.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_4.stl new file mode 100644 index 000000000..9838f2615 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_4.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_5.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_5.stl new file mode 100644 index 000000000..08c186c11 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_5.stl differ diff --git a/kuka_kr10_support/meshes/kr10r1420/visual/link_6.stl b/kuka_kr10_support/meshes/kr10r1420/visual/link_6.stl new file mode 100644 index 000000000..660157052 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r1420/visual/link_6.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/base_link.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/base_link.stl new file mode 100644 index 000000000..ea57f6e08 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/base_link.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_1.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_1.stl new file mode 100644 index 000000000..6af238e4c Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_1.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_2.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_2.stl new file mode 100644 index 000000000..7f2eae0f1 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_2.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_3.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_3.stl new file mode 100644 index 000000000..892a4a7dc Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_3.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_4.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_4.stl new file mode 100644 index 000000000..0c702aa34 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_4.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_5.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_5.stl new file mode 100644 index 000000000..d0b45ab74 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_5.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/collision/link_6.stl b/kuka_kr10_support/meshes/kr10r900_2/collision/link_6.stl new file mode 100644 index 000000000..3e2a49c89 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/collision/link_6.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/base_link.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/base_link.stl new file mode 100644 index 000000000..30f33e110 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/base_link.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_1.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_1.stl new file mode 100644 index 000000000..463882013 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_1.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_2.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_2.stl new file mode 100644 index 000000000..6e25c09e1 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_2.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_3.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_3.stl new file mode 100644 index 000000000..a239e3a68 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_3.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_4.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_4.stl new file mode 100644 index 000000000..cb778df8b Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_4.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_5.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_5.stl new file mode 100644 index 000000000..d40de28fb Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_5.stl differ diff --git a/kuka_kr10_support/meshes/kr10r900_2/visual/link_6.stl b/kuka_kr10_support/meshes/kr10r900_2/visual/link_6.stl new file mode 100644 index 000000000..c5266dbe7 Binary files /dev/null and b/kuka_kr10_support/meshes/kr10r900_2/visual/link_6.stl differ diff --git a/kuka_kr10_support/package.xml b/kuka_kr10_support/package.xml index 464108fe7..81fb381e0 100644 --- a/kuka_kr10_support/package.xml +++ b/kuka_kr10_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr10_support 0.1.0 @@ -9,11 +10,22 @@

This package contains configuration data, 3D models and launch files for KUKA KR 10 manipulators. This currently includes the R1100 sixx only. + From Agilus-2 series it currently includes the R900-2 only.

Specifications:

    +
  • KR 10 R900-2 - Default
  • KR 10 R1100 sixx - Default
    • +
  • KR 10 R1420 - Default
+

+ Joint limits and maximum joint velocities for the Agilus-2 variants + are based on the information in the Kuka Deutschland GmbH - Kuka Robots + KR AGILUS-2 Spezifikation version Spez KR AGILUS-2 V4, 29.01.2019. + All urdfs are based on the default motion and joint velocity limits, + unless noted otherwise (ie: no support for high speed joints, + extended / limited motion ranges or other options). +

Joint limits and maximum joint velocities are based on the information in the KUKA Roboter GmbH - KR AGILUS sixx - With W and C Variants - @@ -22,13 +34,23 @@ unless noted otherwise (ie: no support for high speed joints, extended / limited motion ranges or other options).

+

+ Joint limits and maximum joint velocities are based on the information + in the KUKA Roboter GmbH - KR CYBERTECH nano version + Spez KR CYBERTECH nano V1, 25.07.2016. + All urdfs are based on the default motion and joint velocity limits, + unless noted otherwise (ie: no support for high speed joints, + extended / limited motion ranges or other options). +

Before using any of the configuration files and / or meshes included in this package, be sure to check they are correct for the particular robot model and configuration you intend to use them with.

+ Thibault Brevet (AATB GmbH) Christopher Schindlbeck (University of Hanover) + Simon Schmeisser (Optonic GmbH) G.A. vd. Hoorn (TU Delft Robotics Institute) BSD @@ -36,18 +58,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr10_support/test/roslaunch_test_kr10r1420.xml b/kuka_kr10_support/test/roslaunch_test_kr10r1420.xml new file mode 100644 index 000000000..d0a871057 --- /dev/null +++ b/kuka_kr10_support/test/roslaunch_test_kr10r1420.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/kuka_kr10_support/test/roslaunch_test_kr10r900_2.xml b/kuka_kr10_support/test/roslaunch_test_kr10r900_2.xml new file mode 100644 index 000000000..3b69e0d93 --- /dev/null +++ b/kuka_kr10_support/test/roslaunch_test_kr10r900_2.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/kuka_kr10_support/urdf/kr10r1100sixx.xacro b/kuka_kr10_support/urdf/kr10r1100sixx.xacro index a3a7e2e6f..2567da84c 100644 --- a/kuka_kr10_support/urdf/kr10r1100sixx.xacro +++ b/kuka_kr10_support/urdf/kr10r1100sixx.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro index dee4b6065..c1e3f017c 100644 --- a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro +++ b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro @@ -1,163 +1,170 @@ - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + - - + + - + - + - + - + - + - + - + - - + + @@ -167,5 +174,13 @@ + + + + + + + + diff --git a/kuka_kr10_support/urdf/kr10r1420.xacro b/kuka_kr10_support/urdf/kr10r1420.xacro new file mode 100644 index 000000000..a99a35fdf --- /dev/null +++ b/kuka_kr10_support/urdf/kr10r1420.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/kuka_kr10_support/urdf/kr10r1420_macro.xacro b/kuka_kr10_support/urdf/kr10r1420_macro.xacro new file mode 100644 index 000000000..77c672867 --- /dev/null +++ b/kuka_kr10_support/urdf/kr10r1420_macro.xacro @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_kr10_support/urdf/kr10r900_2.xacro b/kuka_kr10_support/urdf/kr10r900_2.xacro new file mode 100644 index 000000000..1e99a5ea5 --- /dev/null +++ b/kuka_kr10_support/urdf/kr10r900_2.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/kuka_kr10_support/urdf/kr10r900_2_macro.xacro b/kuka_kr10_support/urdf/kr10r900_2_macro.xacro new file mode 100644 index 000000000..20b9660f1 --- /dev/null +++ b/kuka_kr10_support/urdf/kr10r900_2_macro.xacro @@ -0,0 +1,194 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_kr120_support/CMakeLists.txt b/kuka_kr120_support/CMakeLists.txt index 55c5a0b93..a486674ab 100644 --- a/kuka_kr120_support/CMakeLists.txt +++ b/kuka_kr120_support/CMakeLists.txt @@ -1,15 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) +project(kuka_kr120_support LANGUAGES CXX) -project(kuka_kr120_support) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) -catkin_package() -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test.xml) -endif() +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_package() diff --git a/kuka_kr120_support/config/joint_names_kr120r2500pro.yaml b/kuka_kr120_support/config/joint_names_kr120r2500pro.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr120_support/config/joint_names_kr120r2500pro.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr120_support/launch/load_kr120r2500pro.launch b/kuka_kr120_support/launch/load_kr120r2500pro.launch deleted file mode 100644 index 6819876c4..000000000 --- a/kuka_kr120_support/launch/load_kr120r2500pro.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/kuka_kr120_support/launch/test_kr120r2500pro.launch b/kuka_kr120_support/launch/test_kr120r2500pro.launch deleted file mode 100644 index 824935424..000000000 --- a/kuka_kr120_support/launch/test_kr120r2500pro.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/kuka_kr120_support/launch/test_kr120r2500pro.launch.py b/kuka_kr120_support/launch/test_kr120r2500pro.launch.py new file mode 100644 index 000000000..98867cf75 --- /dev/null +++ b/kuka_kr120_support/launch/test_kr120r2500pro.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr120_support", + "robot_description_file": "kr120r2500pro.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr120_support/package.xml b/kuka_kr120_support/package.xml index e03d496ff..c0e1db97c 100644 --- a/kuka_kr120_support/package.xml +++ b/kuka_kr120_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr120_support 0.1.0 @@ -37,17 +38,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr120_support/urdf/kr120r2500pro.urdf b/kuka_kr120_support/urdf/kr120r2500pro.urdf index 925fe1545..72333ec90 100644 --- a/kuka_kr120_support/urdf/kr120r2500pro.urdf +++ b/kuka_kr120_support/urdf/kr120r2500pro.urdf @@ -193,4 +193,3 @@ - diff --git a/kuka_kr120_support/urdf/kr120r2500pro.xacro b/kuka_kr120_support/urdf/kr120r2500pro.xacro index a07102658..16f658d13 100644 --- a/kuka_kr120_support/urdf/kr120r2500pro.xacro +++ b/kuka_kr120_support/urdf/kr120r2500pro.xacro @@ -1,6 +1,9 @@ + + + diff --git a/kuka_kr120_support/urdf/kr120r2500pro_macro.xacro b/kuka_kr120_support/urdf/kr120r2500pro_macro.xacro index 1a0dcedcf..9715df28e 100644 --- a/kuka_kr120_support/urdf/kr120r2500pro_macro.xacro +++ b/kuka_kr120_support/urdf/kr120r2500pro_macro.xacro @@ -1,8 +1,6 @@ - - @@ -15,13 +13,13 @@ - + - + @@ -35,13 +33,13 @@ - + - + @@ -55,13 +53,13 @@ - + - + @@ -75,13 +73,13 @@ - + - + @@ -95,13 +93,13 @@ - + - + @@ -115,13 +113,13 @@ - + - + @@ -135,19 +133,18 @@ - + - + - - - + + @@ -157,7 +154,7 @@ - + @@ -165,7 +162,7 @@ - + @@ -173,7 +170,7 @@ - + @@ -181,7 +178,7 @@ - + @@ -189,7 +186,7 @@ - + @@ -197,13 +194,13 @@ - + - + - - + + @@ -215,7 +212,12 @@ + + + + + + + - - diff --git a/kuka_kr150_support/CMakeLists.txt b/kuka_kr150_support/CMakeLists.txt index a7841a2a5..7faeecd71 100644 --- a/kuka_kr150_support/CMakeLists.txt +++ b/kuka_kr150_support/CMakeLists.txt @@ -1,15 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) -project(kuka_kr150_support) +project(kuka_kr150_support LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test.xml) -endif() -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + DIRECTORY config launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/kuka_kr150_support/config/joint_names_kr150.yaml b/kuka_kr150_support/config/joint_names_kr150.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr150_support/config/joint_names_kr150.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr150_support/config/opw_parameters_kr150r3100_2.yaml b/kuka_kr150_support/config/opw_parameters_kr150r3100_2.yaml new file mode 100644 index 000000000..0fe9947c6 --- /dev/null +++ b/kuka_kr150_support/config/opw_parameters_kr150r3100_2.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.33 + a2: -0.115 + b: 0.0 + c1: 0.645 + c2: 1.35 + c3: 1.42 + c4: 0.215 +opw_kinematics_joint_offsets: [0.0, deg(-90.0), 0.0, 0.0, 0.0, 0.0] +opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] diff --git a/kuka_kr150_support/launch/load_kr150_2.launch b/kuka_kr150_support/launch/load_kr150_2.launch deleted file mode 100644 index c60bd0573..000000000 --- a/kuka_kr150_support/launch/load_kr150_2.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/kuka_kr150_support/launch/test_kr150_2.launch b/kuka_kr150_support/launch/test_kr150_2.launch deleted file mode 100644 index 25e1eacdb..000000000 --- a/kuka_kr150_support/launch/test_kr150_2.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr150_support/launch/test_kr150_2.launch.py b/kuka_kr150_support/launch/test_kr150_2.launch.py new file mode 100644 index 000000000..959b1ec0c --- /dev/null +++ b/kuka_kr150_support/launch/test_kr150_2.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr150_support", + "robot_description_file": "kr150_2.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr150_support/launch/test_kr150r3100_2.launch.py b/kuka_kr150_support/launch/test_kr150r3100_2.launch.py new file mode 100644 index 000000000..6b4b2a2e0 --- /dev/null +++ b/kuka_kr150_support/launch/test_kr150r3100_2.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr150_support", + "robot_description_file": "kr150r3100_2.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/base_link.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/base_link.stl new file mode 100644 index 000000000..43c40bce8 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/base_link.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_1.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_1.stl new file mode 100644 index 000000000..7c80e7fbb Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_1.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_2.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_2.stl new file mode 100644 index 000000000..a39d1192a Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_2.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_3.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_3.stl new file mode 100644 index 000000000..30887c0ab Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_3.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_4.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_4.stl new file mode 100644 index 000000000..225cdba74 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_4.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_5.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_5.stl new file mode 100644 index 000000000..6fa37c485 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_5.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/collision/link_6.stl b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_6.stl new file mode 100644 index 000000000..b9d65c446 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/collision/link_6.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/base_link.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/base_link.stl new file mode 100644 index 000000000..2ea9e2003 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/base_link.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_1.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_1.stl new file mode 100644 index 000000000..43fac2ed1 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_1.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_2.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_2.stl new file mode 100644 index 000000000..9b541cb78 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_2.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_3.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_3.stl new file mode 100644 index 000000000..7a63dacd7 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_3.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_4.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_4.stl new file mode 100644 index 000000000..754a15ecd Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_4.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_5.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_5.stl new file mode 100644 index 000000000..7829651bd Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_5.stl differ diff --git a/kuka_kr150_support/meshes/kr150r3100_2/visual/link_6.stl b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_6.stl new file mode 100644 index 000000000..490516ec4 Binary files /dev/null and b/kuka_kr150_support/meshes/kr150r3100_2/visual/link_6.stl differ diff --git a/kuka_kr150_support/package.xml b/kuka_kr150_support/package.xml index 61141210e..c008785a7 100644 --- a/kuka_kr150_support/package.xml +++ b/kuka_kr150_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr150_support 0.1.0 @@ -14,6 +15,7 @@

Specifications:

  • KR 150-2 - Default
    • +
  • KR 150 R3100-2 - Default

Joint limits and maximum joint velocities are based on the information @@ -21,6 +23,12 @@ All urdfs are based on the default motion and joint velocity limits, unless noted otherwise.

+

+ Joint limits and maximum joint velocities of KR 150 R3100-2 are based on the information + found in the online datasheet KR 150 R3100-2, version 0000-346-515 / V22.1 / 26.03.2021. + All urdfs are based on the default motion and joint velocity limits, + unless noted otherwise. +

Before using any of the configuration files and / or meshes included in this package, be sure to check they are correct for the particular @@ -36,17 +44,18 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro + ros2_control - + ament_cmake diff --git a/kuka_kr150_support/test/roslaunch_test_kr150r3100_2.xml b/kuka_kr150_support/test/roslaunch_test_kr150r3100_2.xml new file mode 100644 index 000000000..b75893525 --- /dev/null +++ b/kuka_kr150_support/test/roslaunch_test_kr150r3100_2.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/kuka_kr150_support/urdf/kr150_2.xacro b/kuka_kr150_support/urdf/kr150_2.xacro index 66f8aafbb..36c67f6b3 100644 --- a/kuka_kr150_support/urdf/kr150_2.xacro +++ b/kuka_kr150_support/urdf/kr150_2.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_kr150_support/urdf/kr150_2_macro.xacro b/kuka_kr150_support/urdf/kr150_2_macro.xacro index 4c4e99faf..23e3f8580 100644 --- a/kuka_kr150_support/urdf/kr150_2_macro.xacro +++ b/kuka_kr150_support/urdf/kr150_2_macro.xacro @@ -1,116 +1,124 @@ + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + - - + + @@ -163,20 +171,28 @@ - + - - + + - - - + + + + + + + + + + + - + diff --git a/kuka_kr150_support/urdf/kr150r3100_2.xacro b/kuka_kr150_support/urdf/kr150r3100_2.xacro new file mode 100644 index 000000000..81dca9019 --- /dev/null +++ b/kuka_kr150_support/urdf/kr150r3100_2.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/kuka_kr150_support/urdf/kr150r3100_2_macro.xacro b/kuka_kr150_support/urdf/kr150r3100_2_macro.xacro new file mode 100644 index 000000000..dce2e4746 --- /dev/null +++ b/kuka_kr150_support/urdf/kr150r3100_2_macro.xacro @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_kr16_support/CMakeLists.txt b/kuka_kr16_support/CMakeLists.txt index 2f7014ae6..a2007e145 100644 --- a/kuka_kr16_support/CMakeLists.txt +++ b/kuka_kr16_support/CMakeLists.txt @@ -1,15 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) -project(kuka_kr16_support) +project(kuka_kr16_support LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test.xml) -endif() -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) + + ament_package() diff --git a/kuka_kr16_support/config/joint_names_kr16.yaml b/kuka_kr16_support/config/joint_names_kr16.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr16_support/config/joint_names_kr16.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr16_support/launch/load_kr16_2.launch b/kuka_kr16_support/launch/load_kr16_2.launch deleted file mode 100644 index 9684d949a..000000000 --- a/kuka_kr16_support/launch/load_kr16_2.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/kuka_kr16_support/launch/test_kr16_2.launch b/kuka_kr16_support/launch/test_kr16_2.launch deleted file mode 100644 index 44260a938..000000000 --- a/kuka_kr16_support/launch/test_kr16_2.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr16_support/launch/test_kr16_2.launch.py b/kuka_kr16_support/launch/test_kr16_2.launch.py new file mode 100644 index 000000000..d0630be1e --- /dev/null +++ b/kuka_kr16_support/launch/test_kr16_2.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr16_support", + "robot_description_file": "kr16_2.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr16_support/package.xml b/kuka_kr16_support/package.xml index 0eff0ceb9..306a9de2a 100644 --- a/kuka_kr16_support/package.xml +++ b/kuka_kr16_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr16_support 0.1.0 @@ -37,17 +38,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr16_support/urdf/kr16_2.urdf b/kuka_kr16_support/urdf/kr16_2.urdf index 7a526d850..b9186b2f0 100644 --- a/kuka_kr16_support/urdf/kr16_2.urdf +++ b/kuka_kr16_support/urdf/kr16_2.urdf @@ -214,4 +214,3 @@ - diff --git a/kuka_kr16_support/urdf/kr16_2.xacro b/kuka_kr16_support/urdf/kr16_2.xacro index 893a73044..0a6cc13b4 100644 --- a/kuka_kr16_support/urdf/kr16_2.xacro +++ b/kuka_kr16_support/urdf/kr16_2.xacro @@ -1,6 +1,9 @@ + + + diff --git a/kuka_kr16_support/urdf/kr16_2_macro.xacro b/kuka_kr16_support/urdf/kr16_2_macro.xacro index 1eb975953..67a8cc742 100644 --- a/kuka_kr16_support/urdf/kr16_2_macro.xacro +++ b/kuka_kr16_support/urdf/kr16_2_macro.xacro @@ -13,13 +13,13 @@ - + - + @@ -33,13 +33,13 @@ - + - + @@ -53,13 +53,13 @@ - + - + @@ -73,13 +73,13 @@ - + - + @@ -93,13 +93,13 @@ - + - + @@ -113,13 +113,13 @@ - + - + @@ -133,19 +133,19 @@ - + - + - - + + @@ -198,10 +198,10 @@ - + - - + + @@ -213,5 +213,13 @@ + + + + + + + + diff --git a/kuka_kr210_support/CMakeLists.txt b/kuka_kr210_support/CMakeLists.txt index 7d7576e84..8e2bd62b7 100644 --- a/kuka_kr210_support/CMakeLists.txt +++ b/kuka_kr210_support/CMakeLists.txt @@ -1,14 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_kr210_support) +cmake_minimum_required(VERSION 3.16) -find_package(catkin REQUIRED) +project(kuka_kr210_support LANGUAGES CXX) -catkin_package() - -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test.xml) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) endif() -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/kuka_kr210_support/config/joint_names_kr210l150.yaml b/kuka_kr210_support/config/joint_names_kr210l150.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr210_support/config/joint_names_kr210l150.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr210_support/launch/load_kr210l150.launch b/kuka_kr210_support/launch/load_kr210l150.launch deleted file mode 100644 index 2b7e8f449..000000000 --- a/kuka_kr210_support/launch/load_kr210l150.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/kuka_kr210_support/launch/test_kr210l150.launch b/kuka_kr210_support/launch/test_kr210l150.launch deleted file mode 100644 index 2e3a9446d..000000000 --- a/kuka_kr210_support/launch/test_kr210l150.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/kuka_kr210_support/launch/test_kr210l150.launch.py b/kuka_kr210_support/launch/test_kr210l150.launch.py new file mode 100644 index 000000000..d09ead067 --- /dev/null +++ b/kuka_kr210_support/launch/test_kr210l150.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr210_support", + "robot_description_file": "kr210l150.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/base_link.stl b/kuka_kr210_support/meshes/kr210r3100/collision/base_link.stl new file mode 100644 index 000000000..93fc4c211 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/base_link.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/base_link2.stl b/kuka_kr210_support/meshes/kr210r3100/collision/base_link2.stl new file mode 100644 index 000000000..ffc84fa9f Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/base_link2.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l1_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l1_1.stl new file mode 100644 index 000000000..56b16b5a1 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l1_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l1_1_2.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l1_1_2.stl new file mode 100644 index 000000000..800b09952 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l1_1_2.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l2_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l2_1.stl new file mode 100644 index 000000000..cbecfd473 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l2_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l3_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l3_1.stl new file mode 100644 index 000000000..160042d0f Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l3_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l4_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l4_1.stl new file mode 100644 index 000000000..2a46ec881 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l4_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l5_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l5_1.stl new file mode 100644 index 000000000..8f9e2f5d4 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l5_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/collision/l6_1.stl b/kuka_kr210_support/meshes/kr210r3100/collision/l6_1.stl new file mode 100644 index 000000000..13208bb30 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/collision/l6_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/base_link.stl b/kuka_kr210_support/meshes/kr210r3100/original/base_link.stl new file mode 100644 index 000000000..83a587061 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/base_link.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l1_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l1_1.stl new file mode 100644 index 000000000..fadb7290c Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l1_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l2_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l2_1.stl new file mode 100644 index 000000000..5f6b7e23c Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l2_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l3_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l3_1.stl new file mode 100644 index 000000000..8d4bf71e0 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l3_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l4_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l4_1.stl new file mode 100644 index 000000000..961de8f7b Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l4_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l5_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l5_1.stl new file mode 100644 index 000000000..b4fca35d8 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l5_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/original/l6_1.stl b/kuka_kr210_support/meshes/kr210r3100/original/l6_1.stl new file mode 100644 index 000000000..4e014c3c6 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/original/l6_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/base_link.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/base_link.stl new file mode 100644 index 000000000..354568623 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/base_link.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l1_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l1_1.stl new file mode 100644 index 000000000..d01903f39 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l1_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l2_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l2_1.stl new file mode 100644 index 000000000..3b0f09264 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l2_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l3_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l3_1.stl new file mode 100644 index 000000000..ee3480c68 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l3_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l4_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l4_1.stl new file mode 100644 index 000000000..3027c339d Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l4_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l5_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l5_1.stl new file mode 100644 index 000000000..70d5653ad Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l5_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/reduced/l6_1.stl b/kuka_kr210_support/meshes/kr210r3100/reduced/l6_1.stl new file mode 100644 index 000000000..71a1e66ac Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/reduced/l6_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/base_link.stl b/kuka_kr210_support/meshes/kr210r3100/visual/base_link.stl new file mode 100644 index 000000000..aa0d50a7e Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/base_link.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l1_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l1_1.stl new file mode 100644 index 000000000..ee27e714b Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l1_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l2_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l2_1.stl new file mode 100644 index 000000000..cbecfd473 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l2_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l3_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l3_1.stl new file mode 100644 index 000000000..388d63d63 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l3_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l4_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l4_1.stl new file mode 100644 index 000000000..28c230f12 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l4_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l5_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l5_1.stl new file mode 100644 index 000000000..a3a42af47 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l5_1.stl differ diff --git a/kuka_kr210_support/meshes/kr210r3100/visual/l6_1.stl b/kuka_kr210_support/meshes/kr210r3100/visual/l6_1.stl new file mode 100644 index 000000000..5d5821b34 Binary files /dev/null and b/kuka_kr210_support/meshes/kr210r3100/visual/l6_1.stl differ diff --git a/kuka_kr210_support/package.xml b/kuka_kr210_support/package.xml index c415a56fa..667a173ba 100644 --- a/kuka_kr210_support/package.xml +++ b/kuka_kr210_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr210_support 0.1.0 @@ -37,17 +38,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr210_support/urdf/kr210l150.urdf b/kuka_kr210_support/urdf/kr210l150.urdf deleted file mode 100644 index 56a478fd2..000000000 --- a/kuka_kr210_support/urdf/kr210l150.urdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kuka_kr210_support/urdf/kr210l150.xacro b/kuka_kr210_support/urdf/kr210l150.xacro index 3dc7d1e08..744d33fb8 100644 --- a/kuka_kr210_support/urdf/kr210l150.xacro +++ b/kuka_kr210_support/urdf/kr210l150.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_kr210_support/urdf/kr210l150_macro.xacro b/kuka_kr210_support/urdf/kr210l150_macro.xacro index 3d4a9f194..2c7778b6d 100644 --- a/kuka_kr210_support/urdf/kr210l150_macro.xacro +++ b/kuka_kr210_support/urdf/kr210l150_macro.xacro @@ -1,8 +1,5 @@ - - - @@ -13,18 +10,18 @@ - + - + - + - + @@ -37,7 +34,7 @@ - + @@ -46,7 +43,7 @@ - + @@ -57,9 +54,9 @@ - + - + @@ -68,7 +65,7 @@ - + @@ -79,9 +76,9 @@ - + - + @@ -90,7 +87,7 @@ - + @@ -101,9 +98,9 @@ - + - + @@ -112,7 +109,7 @@ - + @@ -123,9 +120,9 @@ - + - + @@ -134,7 +131,7 @@ - + @@ -147,7 +144,7 @@ - + @@ -156,12 +153,12 @@ - + - - + + @@ -169,49 +166,65 @@ - + - + - + - + - + - + - + - + + + + + + + + + + + + + + + + + diff --git a/kuka_kr210_support/urdf/kr210r3100.xacro b/kuka_kr210_support/urdf/kr210r3100.xacro new file mode 100644 index 000000000..6326ec3bf --- /dev/null +++ b/kuka_kr210_support/urdf/kr210r3100.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/kuka_kr210_support/urdf/kr210r3100_macro.xacro b/kuka_kr210_support/urdf/kr210r3100_macro.xacro new file mode 100644 index 000000000..be16b2658 --- /dev/null +++ b/kuka_kr210_support/urdf/kr210r3100_macro.xacro @@ -0,0 +1,238 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_kr3_support/CMakeLists.txt b/kuka_kr3_support/CMakeLists.txt index 641d568ad..561663974 100644 --- a/kuka_kr3_support/CMakeLists.txt +++ b/kuka_kr3_support/CMakeLists.txt @@ -1,15 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) -project(kuka_kr3_support) +project(kuka_kr3_support LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test_kr3r540.xml) -endif() -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/kuka_kr3_support/config/joint_names_kr3r540.yaml b/kuka_kr3_support/config/joint_names_kr3r540.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr3_support/config/joint_names_kr3r540.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr3_support/launch/load_kr3r540.launch b/kuka_kr3_support/launch/load_kr3r540.launch deleted file mode 100644 index 93952a27c..000000000 --- a/kuka_kr3_support/launch/load_kr3r540.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/kuka_kr3_support/launch/test_kr3r540.launch b/kuka_kr3_support/launch/test_kr3r540.launch deleted file mode 100644 index a452dc82e..000000000 --- a/kuka_kr3_support/launch/test_kr3r540.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr3_support/launch/test_kr3r540.launch.py b/kuka_kr3_support/launch/test_kr3r540.launch.py new file mode 100644 index 000000000..2cdfe509e --- /dev/null +++ b/kuka_kr3_support/launch/test_kr3r540.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr3_support", + "robot_description_file": "kr3r540.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr3_support/package.xml b/kuka_kr3_support/package.xml index 135bc2f40..9d1578a1b 100644 --- a/kuka_kr3_support/package.xml +++ b/kuka_kr3_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr3_support 0.1.0 @@ -36,18 +37,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr3_support/urdf/kr3r540.xacro b/kuka_kr3_support/urdf/kr3r540.xacro index 32ce1d9ba..7627e58bf 100644 --- a/kuka_kr3_support/urdf/kr3r540.xacro +++ b/kuka_kr3_support/urdf/kr3r540.xacro @@ -1,5 +1,9 @@ + + + + diff --git a/kuka_kr3_support/urdf/kr3r540_macro.xacro b/kuka_kr3_support/urdf/kr3r540_macro.xacro index 8858e1180..aa3088537 100644 --- a/kuka_kr3_support/urdf/kr3r540_macro.xacro +++ b/kuka_kr3_support/urdf/kr3r540_macro.xacro @@ -1,115 +1,124 @@ + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + - - + + + @@ -155,8 +164,8 @@ - - + + @@ -166,5 +175,13 @@ + + + + + + + + diff --git a/kuka_kr5_support/CMakeLists.txt b/kuka_kr5_support/CMakeLists.txt index 69dd5788f..960c65333 100644 --- a/kuka_kr5_support/CMakeLists.txt +++ b/kuka_kr5_support/CMakeLists.txt @@ -1,15 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) -project(kuka_kr5_support) +project(kuka_kr5_support LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test.xml) -endif() +install( +DIRECTORY config launch meshes urdf +DESTINATION share/${PROJECT_NAME} +) -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_package() diff --git a/kuka_kr5_support/config/joint_names_kr5.yaml b/kuka_kr5_support/config/joint_names_kr5.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr5_support/config/joint_names_kr5.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr5_support/config/opw_kinematics_kr5.yaml b/kuka_kr5_support/config/opw_kinematics_kr5.yaml new file mode 100644 index 000000000..4a871f387 --- /dev/null +++ b/kuka_kr5_support/config/opw_kinematics_kr5.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.180 + a2: -0.120 + b: 0.000 + c1: 0.400 + c2: 0.600 + c3: 0.620 + c4: 0.115 +opw_kinematics_joint_offsets: [0.0, -1.57079632679, 0, 0, 0, 0] +opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] diff --git a/kuka_kr5_support/launch/load_kr5_arc.launch b/kuka_kr5_support/launch/load_kr5_arc.launch deleted file mode 100644 index 261c87fe1..000000000 --- a/kuka_kr5_support/launch/load_kr5_arc.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/kuka_kr5_support/launch/test_kr5_arc.launch b/kuka_kr5_support/launch/test_kr5_arc.launch deleted file mode 100644 index c0ac06546..000000000 --- a/kuka_kr5_support/launch/test_kr5_arc.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr5_support/launch/test_kr5_arc.launch.py b/kuka_kr5_support/launch/test_kr5_arc.launch.py new file mode 100644 index 000000000..61449ab7a --- /dev/null +++ b/kuka_kr5_support/launch/test_kr5_arc.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr5_support", + "robot_description_file": "kr5_arc.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr5_support/package.xml b/kuka_kr5_support/package.xml index 12e7ac38e..5309b972c 100644 --- a/kuka_kr5_support/package.xml +++ b/kuka_kr5_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr5_support 0.1.0 @@ -37,17 +38,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr5_support/test/roslaunch_test.xml b/kuka_kr5_support/test/roslaunch_test.xml index e5176c771..9b5126c51 100644 --- a/kuka_kr5_support/test/roslaunch_test.xml +++ b/kuka_kr5_support/test/roslaunch_test.xml @@ -1,9 +1,9 @@ - + - + diff --git a/kuka_kr5_support/urdf/kr5_arc.xacro b/kuka_kr5_support/urdf/kr5_arc.xacro index f54c7434b..fd821b9e2 100644 --- a/kuka_kr5_support/urdf/kr5_arc.xacro +++ b/kuka_kr5_support/urdf/kr5_arc.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_kr5_support/urdf/kr5_arc_macro.xacro b/kuka_kr5_support/urdf/kr5_arc_macro.xacro index 3d8f3f0af..8044b9ef1 100644 --- a/kuka_kr5_support/urdf/kr5_arc_macro.xacro +++ b/kuka_kr5_support/urdf/kr5_arc_macro.xacro @@ -20,13 +20,13 @@ - + - + @@ -49,13 +49,13 @@ - + - + @@ -78,13 +78,13 @@ - + - + @@ -107,13 +107,13 @@ - + - + @@ -136,13 +136,13 @@ - + - + @@ -165,13 +165,13 @@ - + - + @@ -194,25 +194,26 @@ - + - + - - + + + - + @@ -220,7 +221,7 @@ - + @@ -236,7 +237,7 @@ - + @@ -244,7 +245,7 @@ - + @@ -252,27 +253,35 @@ - + - + - - + + - - - + + + - + + + + + + + + + diff --git a/kuka_kr6_support/CMakeLists.txt b/kuka_kr6_support/CMakeLists.txt index 242a41097..8e412f826 100644 --- a/kuka_kr6_support/CMakeLists.txt +++ b/kuka_kr6_support/CMakeLists.txt @@ -1,16 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.16) -project(kuka_kr6_support) +project(kuka_kr6_support LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(test/roslaunch_test_kr6r700sixx.xml) - roslaunch_add_file_check(test/roslaunch_test_kr6r900sixx.xml) -endif() +install( + DIRECTORY config launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) -install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_package() diff --git a/kuka_kr6_support/config/joint_names_kr6r700sixx.yaml b/kuka_kr6_support/config/joint_names_kr6r700sixx.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr6_support/config/joint_names_kr6r700sixx.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr6_support/config/joint_names_kr6r900sixx.yaml b/kuka_kr6_support/config/joint_names_kr6r900sixx.yaml deleted file mode 100644 index 1a23ced85..000000000 --- a/kuka_kr6_support/config/joint_names_kr6r900sixx.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] diff --git a/kuka_kr6_support/config/opw_kinematics_kr6r700sixx.yaml b/kuka_kr6_support/config/opw_kinematics_kr6r700sixx.yaml new file mode 100644 index 000000000..8261db982 --- /dev/null +++ b/kuka_kr6_support/config/opw_kinematics_kr6r700sixx.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.025 + a2: -0.035 + b: 0.000 + c1: 0.400 + c2: 0.315 + c3: 0.365 + c4: 0.080 +opw_kinematics_joint_offsets: [0.0, -1.57079632679, 0, 0, 0, 0] +opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] diff --git a/kuka_kr6_support/config/opw_parameters_kr6r900_2.yaml b/kuka_kr6_support/config/opw_parameters_kr6r900_2.yaml new file mode 100644 index 000000000..ef9ea571e --- /dev/null +++ b/kuka_kr6_support/config/opw_parameters_kr6r900_2.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.025 + a2: -0.025 + b: 0.000 + c1: 0.400 + c2: 0.455 + c3: 0.420 + c4: 0.090 +opw_kinematics_joint_offsets: [0.0, deg(-90.0), 0.0, 0.0, 0.0, 0.0] +opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] diff --git a/kuka_kr6_support/launch/load_kr6r700sixx.launch b/kuka_kr6_support/launch/load_kr6r700sixx.launch deleted file mode 100644 index f579cdd36..000000000 --- a/kuka_kr6_support/launch/load_kr6r700sixx.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/kuka_kr6_support/launch/load_kr6r900sixx.launch b/kuka_kr6_support/launch/load_kr6r900sixx.launch deleted file mode 100644 index 7c276c7dc..000000000 --- a/kuka_kr6_support/launch/load_kr6r900sixx.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/kuka_kr6_support/launch/test_kr6r700sixx.launch b/kuka_kr6_support/launch/test_kr6r700sixx.launch deleted file mode 100644 index 5f2c968e9..000000000 --- a/kuka_kr6_support/launch/test_kr6r700sixx.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr6_support/launch/test_kr6r700sixx.launch.py b/kuka_kr6_support/launch/test_kr6r700sixx.launch.py new file mode 100644 index 000000000..6748d396f --- /dev/null +++ b/kuka_kr6_support/launch/test_kr6r700sixx.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr6_support", + "robot_description_file": "kr6r700sixx.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr6_support/launch/test_kr6r900_2.launch.py b/kuka_kr6_support/launch/test_kr6r900_2.launch.py new file mode 100644 index 000000000..7898360bd --- /dev/null +++ b/kuka_kr6_support/launch/test_kr6r900_2.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr6_support", + "robot_description_file": "kr6r900_2.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr6_support/launch/test_kr6r900sixx.launch b/kuka_kr6_support/launch/test_kr6r900sixx.launch deleted file mode 100644 index 24ea4eaf2..000000000 --- a/kuka_kr6_support/launch/test_kr6r900sixx.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/kuka_kr6_support/launch/test_kr6r900sixx.launch.py b/kuka_kr6_support/launch/test_kr6r900sixx.launch.py new file mode 100644 index 000000000..3099623f7 --- /dev/null +++ b/kuka_kr6_support/launch/test_kr6r900sixx.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_kr6_support", + "robot_description_file": "kr6r900sixx.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_kr6_support/package.xml b/kuka_kr6_support/package.xml index 759c973b5..155a57ac7 100644 --- a/kuka_kr6_support/package.xml +++ b/kuka_kr6_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_kr6_support 0.1.0 @@ -8,18 +9,21 @@

This package contains configuration data, 3D models and launch files - for KUKA KR 6 manipulators. This currently includes the R700 sixx and - the R900 sixx. + for KUKA KR 6 manipulators. This currently includes the R700 sixx, + the R900 sixx and the R900-2.

Specifications:

  • KR 6 R700 sixx - Default
  • KR 6 R900 sixx - Default
    • +
  • KR 6 R900-2 - Default

- Joint limits and maximum joint velocities are based on the information + Joint limits and maximum joint velocities for sixx models are based on the information in the KUKA Roboter GmbH - KR AGILUS sixx - With W and C Variants - Specification version Spez KR AGILUS sixx V12, 26.03.2015. + Joint limits and maximum joint velocities for R900-2 are based on the information + in the KUKA Deutschland GmbH - Robots KR AGILUS-2 Specification version V4, 26.02.2019. All urdfs are based on the default motion and joint velocity limits, unless noted otherwise (ie: no support for high speed joints, extended / limited motion ranges or other options). @@ -29,6 +33,9 @@ in this package, be sure to check they are correct for the particular robot model and configuration you intend to use them with.

+

+ Note: the KR6 R900-2 urdf reuses meshes from the kuka_kr10_support package because of their identical appearance. +

G.A. vd. Hoorn (TU Delft Robotics Institute) Brett Hemes (3M) @@ -39,18 +46,16 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin - - roslaunch + ament_cmake - industrial_robot_client joint_state_publisher + kuka_kr10_support kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_kr6_support/test/roslaunch_test_kr6r900_2.xml b/kuka_kr6_support/test/roslaunch_test_kr6r900_2.xml new file mode 100644 index 000000000..96aeb6c30 --- /dev/null +++ b/kuka_kr6_support/test/roslaunch_test_kr6r900_2.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/kuka_kr6_support/urdf/kr6r700sixx.xacro b/kuka_kr6_support/urdf/kr6r700sixx.xacro index a69371d81..c50cc7fb2 100644 --- a/kuka_kr6_support/urdf/kr6r700sixx.xacro +++ b/kuka_kr6_support/urdf/kr6r700sixx.xacro @@ -1,6 +1,8 @@ - + + + diff --git a/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro b/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro index d1c947022..ddea62587 100644 --- a/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro +++ b/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro @@ -1,163 +1,171 @@ + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + - - + + - + - + - + - + - + - + - + - - + + @@ -167,5 +175,13 @@ + + + + + + + + diff --git a/kuka_kr6_support/urdf/kr6r900_2.xacro b/kuka_kr6_support/urdf/kr6r900_2.xacro new file mode 100644 index 000000000..5ac3e9208 --- /dev/null +++ b/kuka_kr6_support/urdf/kr6r900_2.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/kuka_kr6_support/urdf/kr6r900_2_macro.xacro b/kuka_kr6_support/urdf/kr6r900_2_macro.xacro new file mode 100644 index 000000000..b083e452d --- /dev/null +++ b/kuka_kr6_support/urdf/kr6r900_2_macro.xacro @@ -0,0 +1,188 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_kr6_support/urdf/kr6r900sixx.xacro b/kuka_kr6_support/urdf/kr6r900sixx.xacro index bea136db0..7f435c749 100644 --- a/kuka_kr6_support/urdf/kr6r900sixx.xacro +++ b/kuka_kr6_support/urdf/kr6r900sixx.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro b/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro index e44dd7af4..e33076883 100644 --- a/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro +++ b/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro @@ -2,162 +2,170 @@ + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + - - + + - + - + - + - + - + - + - + - - + + @@ -167,5 +175,13 @@ + + + + + + + + diff --git a/kuka_lbr_iiwa_support/CMakeLists.txt b/kuka_lbr_iiwa_support/CMakeLists.txt index e7b6a5f21..cd98968d7 100644 --- a/kuka_lbr_iiwa_support/CMakeLists.txt +++ b/kuka_lbr_iiwa_support/CMakeLists.txt @@ -1,23 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_lbr_iiwa_support) +cmake_minimum_required(VERSION 3.16) -find_package(catkin REQUIRED) +project(kuka_lbr_iiwa_support LANGUAGES CXX) -catkin_package() +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -install(DIRECTORY config launch urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +find_package(ament_cmake REQUIRED) + + +install( + DIRECTORY launch urdf + DESTINATION share/${PROJECT_NAME} +) # temporarily: until sources are moved to other org/rep install( DIRECTORY meshes/lbr_iiwa_14_r820/collision DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/lbr_iiwa_14_r820/) + share/${PROJECT_NAME}/meshes/lbr_iiwa_14_r820/ +) # temporarily: until sources are moved to other org/rep install( DIRECTORY meshes/lbr_iiwa_14_r820/visual DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/lbr_iiwa_14_r820/) + share/${PROJECT_NAME}/meshes/lbr_iiwa_14_r820/ +) + +ament_package() diff --git a/kuka_lbr_iiwa_support/config/joint_names_lbr_iiwa.yaml b/kuka_lbr_iiwa_support/config/joint_names_lbr_iiwa.yaml deleted file mode 100644 index 299e97a96..000000000 --- a/kuka_lbr_iiwa_support/config/joint_names_lbr_iiwa.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6', 'joint_a7'] diff --git a/kuka_lbr_iiwa_support/launch/load_lbr_iiwa_14_r820.launch b/kuka_lbr_iiwa_support/launch/load_lbr_iiwa_14_r820.launch deleted file mode 100644 index 7e3a2821b..000000000 --- a/kuka_lbr_iiwa_support/launch/load_lbr_iiwa_14_r820.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch b/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch deleted file mode 100644 index 1e9883705..000000000 --- a/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch.py b/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch.py new file mode 100644 index 000000000..0944c0586 --- /dev/null +++ b/kuka_lbr_iiwa_support/launch/test_lbr_iiwa_14_r820.launch.py @@ -0,0 +1,38 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + load_and_test = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "launch", "test_kuka.launch.py"] + ) + ), + launch_arguments={ + "robot_description_package": "kuka_lbr_iiwa_support", + "robot_description_file": "lbr_iiwa_14_r820.xacro", + }.items(), + ) + + launch_files_to_include = [load_and_test] + + return LaunchDescription(launch_files_to_include) diff --git a/kuka_lbr_iiwa_support/package.xml b/kuka_lbr_iiwa_support/package.xml index dd876429e..a3501f6fd 100644 --- a/kuka_lbr_iiwa_support/package.xml +++ b/kuka_lbr_iiwa_support/package.xml @@ -1,5 +1,6 @@ - + + kuka_lbr_iiwa_support 0.1.0 @@ -12,9 +13,9 @@

Joint limits and max joint velocities are based on the information in - the KUKA data sheets online. - All urdf's / xacro's are based on the default motion and joint velocity - limits, unless noted otherwise (ie: no support for high speed joints, + the KUKA data sheets online. + All urdf's / xacro's are based on the default motion and joint velocity + limits, unless noted otherwise (ie: no support for high speed joints, extended / limited motion ranges or other options).

@@ -33,17 +34,17 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - roslaunch + ros2launch - industrial_robot_client joint_state_publisher + kuka_resources robot_state_publisher - rviz + rviz2 xacro - + ament_cmake diff --git a/kuka_lbr_iiwa_support/urdf/kuka_lbr_iiwa_macro.ros2_control.xacro b/kuka_lbr_iiwa_support/urdf/kuka_lbr_iiwa_macro.ros2_control.xacro new file mode 100644 index 000000000..2798b6425 --- /dev/null +++ b/kuka_lbr_iiwa_support/urdf/kuka_lbr_iiwa_macro.ros2_control.xacro @@ -0,0 +1,81 @@ + + + + + + + + + + mock_components/GenericSystem + + + + + + -1 + 1 + + + 0.0 + + + + + -1 + 1 + + + 0.0 + + + + + -1 + 0.61 + + + 0.0 + + + + + -1 + 1 + + + 0.0 + + + + + -1 + 1 + + + 0.0 + + + + + -1 + 1 + + + 0.0 + + + + + -1 + 1 + + + 0.0 + + + + + + + diff --git a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.urdf b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.urdf index e8eb8028a..7c1436e39 100644 --- a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.urdf +++ b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.urdf @@ -184,4 +184,3 @@ - diff --git a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.xacro b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.xacro index 5f2facff2..51693de73 100644 --- a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.xacro +++ b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.xacro @@ -1,5 +1,8 @@ + + + diff --git a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820_macro.xacro b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820_macro.xacro index 40bf9028b..10c6175d9 100644 --- a/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820_macro.xacro +++ b/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820_macro.xacro @@ -1,116 +1,126 @@ + + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + + - + - + @@ -184,4 +194,3 @@ - diff --git a/kuka_resources/CMakeLists.txt b/kuka_resources/CMakeLists.txt index 89269dd25..deabc1ac0 100644 --- a/kuka_resources/CMakeLists.txt +++ b/kuka_resources/CMakeLists.txt @@ -1,9 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_resources) +cmake_minimum_required(VERSION 3.16) -find_package(catkin REQUIRED) +project(kuka_resources LANGUAGES CXX) -catkin_package() +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -install(DIRECTORY urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +find_package(ament_cmake REQUIRED) + + +install( + DIRECTORY config launch rviz urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/kuka_resources/config/initial_positions.yaml b/kuka_resources/config/initial_positions.yaml new file mode 100644 index 000000000..1381d92d9 --- /dev/null +++ b/kuka_resources/config/initial_positions.yaml @@ -0,0 +1,9 @@ +# Initial positions for kuka manipulators when using ros2_control mock hardware + +joint_a1: 0 +joint_a2: -1.57 +joint_a3: 1.57 +joint_a4: 0 +joint_a5: 1.57 +joint_a6: 0 +joint_a7: 0 # for 7dof robots: lbr_iiwa_14_r820 diff --git a/kuka_resources/config/initial_positions_all_zeros.yaml b/kuka_resources/config/initial_positions_all_zeros.yaml new file mode 100644 index 000000000..669d21f26 --- /dev/null +++ b/kuka_resources/config/initial_positions_all_zeros.yaml @@ -0,0 +1,9 @@ +# Initial positions for kuka manipulators when using ros2_control mock hardware + +joint_a1: 0 +joint_a2: 0 +joint_a3: 0 +joint_a4: 0 +joint_a5: 0 +joint_a6: 0 +joint_a7: 0 # for 7dof robots: lbr_iiwa_14_r820 diff --git a/kuka_resources/config/kuka_6dof_controllers.yaml b/kuka_resources/config/kuka_6dof_controllers.yaml new file mode 100644 index 000000000..93ce205aa --- /dev/null +++ b/kuka_resources/config/kuka_6dof_controllers.yaml @@ -0,0 +1,49 @@ +controller_manager: + ros__parameters: + update_rate: 250 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +forward_position_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + interface_name: position + +position_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_resources/config/kuka_7dof_controllers.yaml b/kuka_resources/config/kuka_7dof_controllers.yaml new file mode 100644 index 000000000..0f8574c19 --- /dev/null +++ b/kuka_resources/config/kuka_7dof_controllers.yaml @@ -0,0 +1,49 @@ +controller_manager: + ros__parameters: + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +forward_position_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + - joint_a7 + interface_name: position + +position_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + - joint_a7 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_resources/launch/test_kuka.launch.py b/kuka_resources/launch/test_kuka.launch.py new file mode 100644 index 000000000..975134146 --- /dev/null +++ b/kuka_resources/launch/test_kuka.launch.py @@ -0,0 +1,83 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_description_package", + default_value="kuka_kr5_support", + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_description_file", + default_value="kr5_arc.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + + # initialize arguments + robot_description_package = LaunchConfiguration("robot_description_package") + robot_description_file = LaunchConfiguration("robot_description_file") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(robot_description_package), "urdf", robot_description_file] + ), + ] + ) + + robot_description = {"robot_description": robot_description_content} + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "config", "view_robot.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + nodes = [joint_state_publisher_node, robot_state_publisher_node, rviz_node] + + return LaunchDescription(declared_arguments + nodes) diff --git a/kuka_resources/package.xml b/kuka_resources/package.xml index eec17c69d..ee7c90673 100644 --- a/kuka_resources/package.xml +++ b/kuka_resources/package.xml @@ -1,5 +1,6 @@ - + + kuka_resources 0.1.0 @@ -19,9 +20,20 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake + + joint_state_publisher + joint_state_broadcaster + robot_state_publisher + joint_state_publisher_gui + controller_manager + forward_command_controller + ros2_control + ros2_controllers + rviz2 + xacro - + ament_cmake diff --git a/kuka_resources/rviz/view_robot.rviz b/kuka_resources/rviz/view_robot.rviz new file mode 100644 index 000000000..0297f3aa0 --- /dev/null +++ b/kuka_resources/rviz/view_robot.rviz @@ -0,0 +1,249 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1063 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + flange: + Value: true + link_1: + Value: true + link_2: + Value: true + link_3: + Value: true + link_4: + Value: true + link_5: + Value: true + link_6: + Value: true + tool0: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + base: + {} + link_1: + link_2: + link_3: + link_4: + link_5: + link_6: + flange: + tool0: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.086756229400635 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.17583326995372772 + Y: 0.44009995460510254 + Z: 0.5812957286834717 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5203982591629028 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.123579025268555 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1411 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004cafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000005a000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000049000004ca000000fc01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004cafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000049000004ca000000d101000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000004bfc0100000002fb0000000800540069006d0065010000000000000a00000002bf01000003fb0000000800540069006d0065010000000000000450000000000000000000000784000004ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1440 diff --git a/kuka_resources/urdf/common_colours.xacro b/kuka_resources/urdf/common_colours.xacro index 1d0b3c419..39dd2532d 100644 --- a/kuka_resources/urdf/common_colours.xacro +++ b/kuka_resources/urdf/common_colours.xacro @@ -106,6 +106,12 @@ + + + + + + diff --git a/kuka_resources/urdf/common_materials.xacro b/kuka_resources/urdf/common_materials.xacro index 57ad06e67..5613773bf 100644 --- a/kuka_resources/urdf/common_materials.xacro +++ b/kuka_resources/urdf/common_materials.xacro @@ -182,9 +182,21 @@ + + + + + + + + + + + + diff --git a/kuka_resources/urdf/common_properties.xacro b/kuka_resources/urdf/common_properties.xacro new file mode 100644 index 000000000..31842e43d --- /dev/null +++ b/kuka_resources/urdf/common_properties.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/kuka_ros2_control_support/CMakeLists.txt b/kuka_ros2_control_support/CMakeLists.txt new file mode 100644 index 000000000..8daeb609d --- /dev/null +++ b/kuka_ros2_control_support/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.16) +project(kuka_ros2_control_support LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_manager + rclcpp +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_executable(ros2_control_node_steady_clock src/ros2_control_node_steady_clock.cpp) +target_include_directories(ros2_control_node_steady_clock PUBLIC + $ + $ +) +ament_target_dependencies(ros2_control_node_steady_clock PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +add_executable(ros2_control_node_max_update_rate_sc src/ros2_control_node_max_update_rate_sc.cpp) +target_include_directories(ros2_control_node_max_update_rate_sc PUBLIC + $ + $ +) +ament_target_dependencies(ros2_control_node_max_update_rate_sc PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake) + + add_launch_test(test/ros2_control_support.py TIMEOUT 180) + +endif() + +install( + TARGETS ros2_control_node_steady_clock ros2_control_node_max_update_rate_sc + RUNTIME DESTINATION lib/kuka_ros2_control_support +) + +install( + DIRECTORY config urdf launch + DESTINATION share/kuka_ros2_control_support +) + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/kuka_ros2_control_support/config/6dof_controllers.yaml b/kuka_ros2_control_support/config/6dof_controllers.yaml new file mode 100644 index 000000000..a41cad6b9 --- /dev/null +++ b/kuka_ros2_control_support/config/6dof_controllers.yaml @@ -0,0 +1,36 @@ +controller_manager: + ros__parameters: + update_rate: 250 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +position_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_ros2_control_support/config/6dof_joint_trajectory_controller_goals.yaml b/kuka_ros2_control_support/config/6dof_joint_trajectory_controller_goals.yaml new file mode 100644 index 000000000..b27c8e69e --- /dev/null +++ b/kuka_ros2_control_support/config/6dof_joint_trajectory_controller_goals.yaml @@ -0,0 +1,31 @@ +publisher_joint_trajectory_controller: + ros__parameters: + controller_name: "position_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0] + pos2: + positions: [0.0, -1.57, 1.57, 0.0, -1.57, 0.0] + pos3: + positions: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0] + pos4: + positions: [0.0, -1.57, 1.57, 0.0, 1.57, 0.0] + + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + check_starting_point: true + starting_point_limits: + joint_a1: [-0.1,0.1] + joint_a2: [-1.57,1.57] + joint_a3: [-1.57,1.57] + joint_a4: [-0.1,0.1] + joint_a5: [-2.0,2.0] + joint_a6: [-0.1,0.1] diff --git a/kuka_ros2_control_support/config/7dof_controllers.yaml b/kuka_ros2_control_support/config/7dof_controllers.yaml new file mode 100644 index 000000000..37134f26d --- /dev/null +++ b/kuka_ros2_control_support/config/7dof_controllers.yaml @@ -0,0 +1,37 @@ +controller_manager: + ros__parameters: + update_rate: 250 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +position_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + - joint_a7 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_ros2_control_support/config/kuka_6dof_joint_trajectory_controller_goals.yaml b/kuka_ros2_control_support/config/kuka_6dof_joint_trajectory_controller_goals.yaml new file mode 100644 index 000000000..1e78b0fc9 --- /dev/null +++ b/kuka_ros2_control_support/config/kuka_6dof_joint_trajectory_controller_goals.yaml @@ -0,0 +1,27 @@ +publisher_joint_trajectory_controller: + ros__parameters: + controller_name: "position_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.0, -1.57, 1.57, 0.0, 1.57, 0.0] + pos2: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0] + pos3: [0.0, -1.57, 1.57, 0.0, 1.57, 0.0] + pos4: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0] + + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + check_starting_point: false + starting_point_limits: + joint_a1: [-0.1,0.1] + joint_a2: [-0.1,0.1] + joint_a3: [-0.1,0.1] + joint_a4: [-0.1,0.1] + joint_a5: [-0.1,0.1] + joint_a6: [-0.1,0.1] diff --git a/kuka_ros2_control_support/launch/bringup.launch.py b/kuka_ros2_control_support/launch/bringup.launch.py new file mode 100644 index 000000000..0987867b2 --- /dev/null +++ b/kuka_ros2_control_support/launch/bringup.launch.py @@ -0,0 +1,368 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Univ of CO, Boulder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_name", + default_value="kuka", + description="Name of the robot or application for unique identification.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value="", + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_macro_file", + description="URDF/XACRO description file with of the robot or application. \ + The expected location of the file is '/urdf/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "configuration_package", + default_value="kuka_resources", + description="Package with configuration files, e.g., controllers, initial positions.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="kuka_6dof_controllers.yaml", + description="YAML file with the controllers configuration. \ + The expected location of the file is '/config/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_positions_file", + default_value="initial_positions.yaml", + description="YAML file with the initial positions when using mock hardware from \ + ros2_control. The expected location of the file is '/config/'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_eki_communication", + default_value="false", + description="Use EKI communication to the KUKA Robot Controller (KR-C). \ + If the flag is set to true 'eki_robot_ip' and 'eki_robot_port' arguments define the \ + endpoint of robot controller to connect.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_ip", + default_value="127.0.0.1", + description="IP address for the robot controller for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_port", + default_value="54600", + description="Port by which the robot can be reached for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rsi_communication", + default_value="false", + description="Use RSI communication to the KUKA Robot Controller (KR-C). \ + If the flag is set to true 'rsi_listen_ip' and 'rsi_robot_port' arguments define the \ + endpoint where robot controller should connect to. (Keep in mind that you have to \ + define this endpoint also in the RSI configuration of the program.)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_ip", + default_value="127.0.0.1", + description="IP address of a computer where robot controller should connect for \ + communication using RSI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_port", + default_value="49152", + description="Port on a computer where robot controller should connect for \ + communication using RSI protocol.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rviz_file", + default_value="view_robot.rviz", + description="Rviz2 configuration file of the visualization. \ + The expected location of the file is '/config/'.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_ros2_control", + default_value="true", + description="Decide if this file should also start ros2_control stack and activate\ + controllers. This is useful for testing, but nor advised in production.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_manager_name", + default_value="controller_manager", + description="Name of the controller manager (controller manager node) with namespace.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start vizualization in Rviz2.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "log_level_driver", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "log_level_all", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + + # initialize arguments + robot_name = LaunchConfiguration("robot_name") + prefix = LaunchConfiguration("prefix") + description_package = LaunchConfiguration("description_package") + description_macro_file = LaunchConfiguration("description_macro_file") + configuration_package = LaunchConfiguration("configuration_package") + controllers_file = LaunchConfiguration("controllers_file") + initial_positions_file = LaunchConfiguration("initial_positions_file") + + use_eki_communication = LaunchConfiguration("use_eki_communication") + eki_robot_ip = LaunchConfiguration("eki_robot_ip") + eki_robot_port = LaunchConfiguration("eki_robot_port") + use_rsi_communication = LaunchConfiguration("use_rsi_communication") + rsi_listen_ip = LaunchConfiguration("rsi_listen_ip") + rsi_listen_port = LaunchConfiguration("rsi_listen_port") + + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + + rviz_file = LaunchConfiguration("rviz_file") + start_rviz = LaunchConfiguration("start_rviz") + + log_level_driver = LaunchConfiguration("log_level_driver") + log_level_all = LaunchConfiguration("log_level_all") + + activate_ros2_control = LaunchConfiguration("activate_ros2_control") + controller_manager_name = LaunchConfiguration("controller_manager_name") + + initial_positions_file = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", initial_positions_file] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("kuka_ros2_control_support"), "urdf", "common_kuka.xacro"] + ), + " ", + "robot_name:=", + robot_name, + " ", + "prefix:=", + prefix, + " ", + "description_package:=", + description_package, + " ", + "description_macro_file:=", + description_macro_file, + " ", + "use_eki_communication:=", + use_eki_communication, + " ", + "eki_robot_ip:=", + eki_robot_ip, + " ", + "eki_robot_port:=", + eki_robot_port, + " ", + "use_rsi_communication:=", + use_rsi_communication, + " ", + "rsi_listen_ip:=", + rsi_listen_ip, + " ", + "rsi_listen_port:=", + rsi_listen_port, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "initial_positions_file:=", + initial_positions_file, + ] + ) + robot_description = {"robot_description": robot_description_content} + + # Publish TF + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher_node", + output="both", + parameters=[robot_description], + ) + + # ros2_control_node + robot_controllers = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", controllers_file] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", # optionally use `ros2_control_node` from ros2_control + output="both", + arguments=[ + "--ros-args", + "--log-level", + ["KukaSystemPositionOnlyHardware:=", log_level_driver], + "--ros-args", + "--log-level", + log_level_all, + ], + parameters=[robot_description, robot_controllers], + condition=IfCondition(activate_ros2_control), + ) + + + load_and_activate_controllers = [] + for controller in ["position_trajectory_controller", "joint_state_broadcaster"]: + load_and_activate_controllers += [ + ExecuteProcess( + cmd=[f"ros2 run controller_manager spawner {controller} -c {controller_manager_name}"], + shell=True, + output="screen", + condition=IfCondition(activate_ros2_control), + ) + ] + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(configuration_package), "config", rviz_file] + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(start_rviz), + ) + + nodes = [ + control_node, + robot_state_publisher_node, + rviz_node + ] + + return LaunchDescription(declared_arguments + nodes + load_and_activate_controllers) diff --git a/kuka_ros2_control_support/launch/bringup.launch.xml b/kuka_ros2_control_support/launch/bringup.launch.xml new file mode 100644 index 000000000..674c3b69d --- /dev/null +++ b/kuka_ros2_control_support/launch/bringup.launch.xml @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_ros2_control_support/launch/test_bringup.launch.py b/kuka_ros2_control_support/launch/test_bringup.launch.py new file mode 100644 index 000000000..6aad17d1c --- /dev/null +++ b/kuka_ros2_control_support/launch/test_bringup.launch.py @@ -0,0 +1,251 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Univ of CO, Boulder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="kuka_6dof_controllers.yaml", + choices=[ + "kuka_6dof_controllers.yaml", + "kuka_7dof_controllers.yaml", + ], + description="YAML file with the controllers configuration. Only LBR IIWA 14 has 7 dofs.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_positions_file", + default_value="initial_positions.yaml", + choices=[ + "initial_positions.yaml", + "initial_positions_all_zeros.yaml", + ], + description="YAML file with the initial positions when using mock hardware from \ + ros2_control. Robots using '_all_zeros' file are: \ + - kr210l150 \ + - ", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Description package with robot URDF/xacro files.", + choices=[ + "kuka_kr3_support", + "kuka_kr5_support", + "kuka_kr6_support", + "kuka_kr10_support", + "kuka_kr16_support", + "kuka_kr120_support", + "kuka_kr150_support", + "kuka_kr210_support", + "kuka_lbr_iiwa_support", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_macro_file", + description="URDF/XACRO description file with of the robot or application. \ + The expected location of the file is '/urdf/'.", + choices=[ + "kr3r540_macro.xacro", + "kr5_arc_macro.xacro", + "kr6r700sixx_macro.xacro", + "kr6r900_2_macro.xacro", + "kr6r900sixx_macro.xacro", + "kr10r900_2_macro.xacro", + "kr10r1100sixx_macro.xacro", + "kr10r1420_macro.xacro", + "kr16_2_macro.xacro", + "kr120r2500pro_macro.xacro", + "kr150_2_macro.xacro", + "kr150r3100_2_macro.xacro", + "kr210l150_macro.xacro", + "lbr_iiwa_14_r820_macro.xacro", + ], + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_eki_communication", + default_value="false", + description="Use EKI communication to the KUKA Robot Controller (KR-C). If the flag is set to true 'eki_robot_ip' and 'eki_robot_port' arguments define the endpoint of robot controller to connect.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_ip", + default_value="127.0.0.1", + description="IP address for the robot controller for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "eki_robot_port", + default_value="54600", + description="Port by which the robot can be reached for communication using EKI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rsi_communication", + default_value="false", + description="Use RSI communication to the KUKA Robot Controller (KR-C). If the flag is set to true 'rsi_listen_ip' and 'rsi_robot_port' arguments define the endpoint where robot controller should connect to. (Keep in mind that you have to define this endpoint also in the RSI configuration of the program.)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_ip", + default_value="127.0.0.1", + description="IP address of a computer where robot controller should connect for communication using RSI protocol.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rsi_listen_port", + default_value="49152", + description="Port on a computer where robot controller should connect for communication using RSI protocol.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start vizualization in Rviz2.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "log_level_driver", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "log_level_all", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + + # initialize arguments + controllers_file = LaunchConfiguration("controllers_file") + initial_positions_file = LaunchConfiguration("initial_positions_file") + description_package = LaunchConfiguration("description_package") + description_macro_file = LaunchConfiguration("description_macro_file") + + use_eki_communication = LaunchConfiguration("use_eki_communication") + eki_robot_ip = LaunchConfiguration("eki_robot_ip") + eki_robot_port = LaunchConfiguration("eki_robot_port") + use_rsi_communication = LaunchConfiguration("use_rsi_communication") + rsi_listen_ip = LaunchConfiguration("rsi_listen_ip") + rsi_listen_port = LaunchConfiguration("rsi_listen_port") + + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + + start_rviz = LaunchConfiguration("start_rviz") + + log_level_driver = LaunchConfiguration("log_level_driver") + log_level_all = LaunchConfiguration("log_level_all") + + ros2_control_setup_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("kuka_ros2_control_support"), "launch", "bringup.launch.py"] + ) + ), + launch_arguments={ + "robot_name": "kuka_test_setup", + "controllers_file": controllers_file, + "initial_positions_file": initial_positions_file, + "description_package": description_package, + "description_macro_file": description_macro_file, + "use_eki_communication": use_eki_communication, + "eki_robot_ip": eki_robot_ip, + "eki_robot_port": eki_robot_port, + "use_rsi_communication": use_rsi_communication, + "rsi_listen_ip": rsi_listen_ip, + "rsi_listen_port": rsi_listen_port, + "use_mock_hardware": use_mock_hardware, + "start_rviz": start_rviz, + "log_level_driver": log_level_driver, + "log_level_all": log_level_all, + }.items(), + ) + + return LaunchDescription(declared_arguments + [ros2_control_setup_bringup_launch]) diff --git a/kuka_ros2_control_support/launch/test_joint_trajectory_controller.launch.py b/kuka_ros2_control_support/launch/test_joint_trajectory_controller.launch.py new file mode 100644 index 000000000..6b88771d3 --- /dev/null +++ b/kuka_ros2_control_support/launch/test_joint_trajectory_controller.launch.py @@ -0,0 +1,40 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + position_goals = PathJoinSubstitution( + [ + FindPackageShare("kuka_ros2_control_support"), + "config/", + "6dof_joint_trajectory_controller_goals.yaml", + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/kuka_ros2_control_support/launch/test_ros2_control_kuka.launch.py b/kuka_ros2_control_support/launch/test_ros2_control_kuka.launch.py new file mode 100644 index 000000000..6b77cb4fc --- /dev/null +++ b/kuka_ros2_control_support/launch/test_ros2_control_kuka.launch.py @@ -0,0 +1,303 @@ +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="kuka_6dof_controllers.yaml", + choices=[ + "kuka_6dof_controllers.yaml", + "kuka_7dof_controllers.yaml", + # Note: for the robot kuka_lbr_iiwa_14_r820, kuka_7dof_controllers.yaml should be used + # and the rest use kuka_6dof_controllers.yaml + ], + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_description_package", + choices=[ + "kuka_kr3_support", + "kuka_kr5_support", + "kuka_kr6_support", + "kuka_kr10_support", + "kuka_kr16_support", + "kuka_kr120_support", + "kuka_kr150_support", + "kuka_kr210_support", + "kuka_lbr_iiwa_support", + ], + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_description_macro_file", + choices=[ + "kr3r540_macro.xacro", + "kr5_arc_macro.xacro", + "kr6r700sixx_macro.xacro", + "kr6r900_2_macro.xacro", + "kr6r900sixx_macro.xacro", + "kr10r900_2_macro.xacro", + "kr10r1100sixx_macro.xacro", + "kr10r1420_macro.xacro", + "kr16_2_macro.xacro", + "kr120r2500pro_macro.xacro", + "kr150_2_macro.xacro", + "kr150r3100_2_macro.xacro", + "kr210l150_macro.xacro", + "lbr_iiwa_14_r820_macro.xacro", + ], + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value="", + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_name", + choices=[ + "kuka_kr3r540", + "kuka_kr5_arc", + "kuka_kr6r700sixx", + "kuka_kr6r900_2", + "kuka_kr6r900sixx", + "kuka_kr10r900_2", + "kuka_kr10r1100sixx", + "kuka_kr10r1420", + "kuka_kr16_2", + "kuka_kr120r2500pro", + "kuka_kr150_2", + "kuka_kr150r3100_2", + "kuka_kr210l150", + "kuka_lbr_iiwa_14_r820", + ], + description="NOTE:robot name and robot description macro name are same", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "listen_ip_address", + default_value="127.0.0.1", + description="The ip address on of your device on which is listend.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "listen_port", + default_value="49152", + description="The port on which is listend.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "log_level_driver", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "log_level_all", + default_value="info", + description="Set the logging level of the loggers of all started nodes.", + choices=[ + "debug", + "DEBUG", + "info", + "INFO", + "warn", + "WARN", + "error", + "ERROR", + "fatal", + "FATAL", + ], + ) + ) + + # initialize arguments + controllers_file = LaunchConfiguration("controllers_file") + robot_description_package = LaunchConfiguration("robot_description_package") + robot_description_macro_file = LaunchConfiguration("robot_description_macro_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + robot_name = LaunchConfiguration("robot_name") + listen_ip_address = LaunchConfiguration("listen_ip_address") + listen_port = LaunchConfiguration("listen_port") + log_level_driver = LaunchConfiguration("log_level_driver") + log_level_all = LaunchConfiguration("log_level_all") + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("kuka_ros2_control_support"), + "urdf", + "common_kuka.xacro", + ] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "controllers_file:=", + controllers_file, + " ", + "robot_description_package:=", + robot_description_package, + " ", + "robot_description_macro_file:=", + robot_description_macro_file, + " ", + "robot_name:=", + robot_name, + " ", + "listen_ip_address:=", + listen_ip_address, + " ", + "listen_port:=", + listen_port, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "config", controllers_file] + ) + + control_node = Node( + package="kuka_ros2_control_support", + executable="ros2_control_node_steady_clock", + output="both", + arguments=[ + "--ros-args", + "--log-level", + ["KukaSystemPositionOnlyHardware:=", log_level_driver], + "--ros-args", + "--log-level", + log_level_all, + ], + parameters=[robot_description, robot_controllers], + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description], + output="both", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["position_trajectory_controller", "-c", "/controller_manager"], + ) + + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("kuka_resources"), "config", "view_robot.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/kuka_ros2_control_support/package.xml b/kuka_ros2_control_support/package.xml new file mode 100644 index 000000000..539bf84bf --- /dev/null +++ b/kuka_ros2_control_support/package.xml @@ -0,0 +1,36 @@ + + + + kuka_ros2_control_support + 0.0.0 + File needed to run KUKA robots with ros2_control. + Giridhar Bukka + Dr.-Ing. Denis Štogl + Apache2 + + ament_cmake + + ros2launch + + controller_manager + rclcpp + realtime_tools + + forward_command_controller + joint_state_publisher + joint_state_broadcaster + joint_trajectory_controller + kuka_resources + kuka_rsi_hw_interface + + robot_state_publisher + ros2launch + rviz2 + tinyxml_vendor + + launch_testing_ament_cmake + + + ament_cmake + + diff --git a/kuka_ros2_control_support/src/ros2_control_node_max_update_rate_sc.cpp b/kuka_ros2_control_support/src/ros2_control_node_max_update_rate_sc.cpp new file mode 100644 index 000000000..86a9fa342 --- /dev/null +++ b/kuka_ros2_control_support/src/ros2_control_node_max_update_rate_sc.cpp @@ -0,0 +1,94 @@ +// Copyright 2020 ROS2-Control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/thread_priority.hpp" + +using namespace std::chrono_literals; + +namespace +{ +// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html +// This value is used when configuring the main loop to use SCHED_FIFO scheduling +// We use a midpoint RT priority to allow maximum flexibility to users +int const kSchedPriority = 50; + +} // namespace + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr executor = + std::make_shared(); + std::string manager_node_name = "controller_manager"; + rclcpp::NodeOptions node_options; + // Required for getting types of controllers to be loaded via service call + node_options.allow_undeclared_parameters(true); + node_options.automatically_declare_parameters_from_overrides(true); + //node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + + auto cm = std::make_shared( + executor, manager_node_name, "", node_options); + + RCLCPP_INFO_STREAM( + rclcpp::get_logger("Clock_Info"), "Clock type:" << cm->get_clock()->get_clock_type()); + + RCLCPP_WARN(cm->get_logger(), "Update rate not used! Updating with maximum possible rate."); + + std::thread cm_thread( + [cm]() + { + if (realtime_tools::has_realtime_kernel()) + { + if (!realtime_tools::configure_sched_fifo(kSchedPriority)) + { + RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy"); + } + } + else + { + RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance"); + } + + // for calculating the measured period of the loop + rclcpp::Time previous_time = cm->now(); + + while (rclcpp::ok()) + { + // calculate measured period + auto const current_time = cm->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; + + // execute update loop + cm->read(cm->now(), measured_period); + cm->update(cm->now(), measured_period); + cm->write(cm->now(), measured_period); + } + }); + + executor->add_node(cm); + executor->spin(); + cm_thread.join(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/kuka_ros2_control_support/src/ros2_control_node_steady_clock.cpp b/kuka_ros2_control_support/src/ros2_control_node_steady_clock.cpp new file mode 100644 index 000000000..7f4299113 --- /dev/null +++ b/kuka_ros2_control_support/src/ros2_control_node_steady_clock.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_helpers.hpp" + +using namespace std::chrono_literals; + +namespace +{ +// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html +// This value is used when configuring the main loop to use SCHED_FIFO scheduling +// We use a midpoint RT priority to allow maximum flexibility to users +int const kSchedPriority = 50; + +} // namespace + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr executor = + std::make_shared(); + std::string manager_node_name = "controller_manager"; + rclcpp::NodeOptions node_options; + // Required for getting types of controllers to be loaded via service call + node_options.allow_undeclared_parameters(true); + node_options.automatically_declare_parameters_from_overrides(true); + // node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + + auto cm = std::make_shared( + executor, manager_node_name, "", node_options); + + RCLCPP_WARN( + cm->get_logger(), + "Update rate not used! Updating with maximum possible rate. Synchronization is achieved via " + "RSI's read function."); + + std::thread cm_thread( + [cm]() + { + if (realtime_tools::has_realtime_kernel()) + { + if (!realtime_tools::configure_sched_fifo(kSchedPriority)) + { + RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy"); + } + } + else + { + RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance"); + } + + // for calculating the measured period of the loop + rclcpp::Time previous_time = cm->now(); + + while (rclcpp::ok()) + { + // calculate measured period + auto const current_time = cm->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; + + // execute update loop + cm->read(cm->now(), measured_period); + cm->update(cm->now(), measured_period); + cm->write(cm->now(), measured_period); + } + }); + + executor->add_node(cm); + executor->spin(); + cm_thread.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/kuka_ros2_control_support/test/ros2_control_support.py b/kuka_ros2_control_support/test/ros2_control_support.py new file mode 100755 index 000000000..3a245d2e1 --- /dev/null +++ b/kuka_ros2_control_support/test/ros2_control_support.py @@ -0,0 +1,353 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Univ of CO, Boulder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER 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. + + +import time +import unittest +from dataclasses import dataclass +from typing import List + +import launch_testing +import pytest +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.msg import JointTrajectoryControllerState +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from geometry_msgs.msg import Vector3 +from rclpy.node import Node +from rclpy.wait_for_message import wait_for_message +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +@dataclass +class KukaArgCombination: + """Class for keeping track of arguments for testing ros2 control support package.""" + + description_package: str + description_macro_file: str + robot_name: str + controllers_file: str + init_pos_file: str + + +def get_launch_arg_choices(package_name: str, launch_file_name: str) -> dict: + launch_description_source = PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(package_name), "launch", launch_file_name]) + ) + launch_description = launch_description_source.try_get_launch_description_without_context() + return {arg.name: arg.choices for arg in launch_description.get_launch_arguments()} + + +def get_possible_combinations( + package_name: str, + launch_file_name: str, + robot_names_7dof: List[str], + description_package_arg_name: str, + description_macro_file_arg_name: str, + controllers_file_arg_name: str, + initial_positions_file_arg_name: str, + robot_names_init_pos_all_zeros: str, +) -> List[KukaArgCombination]: + launch_arg_choices = get_launch_arg_choices(package_name, launch_file_name) + description_package_choices = launch_arg_choices[description_package_arg_name] + description_macro_file_choices = launch_arg_choices[description_macro_file_arg_name] + controllers_file_choices = launch_arg_choices[controllers_file_arg_name] + initial_positions_file_choices = launch_arg_choices[initial_positions_file_arg_name] + + if len(controllers_file_choices) != 2: + raise ValueError("this script accepts exactly 2 controller files") + + if "6dof" in controllers_file_choices[0]: + controllers_file_6dof, controllers_file_7dof = controllers_file_choices + else: + controllers_file_7dof, controllers_file_6dof = controllers_file_choices + + if len(initial_positions_file_choices) != 2: + raise ValueError("this script accepts exactly 2 initial positions files") + + if "all_zeros" in initial_positions_file_choices[0]: + init_pos_all_zeros, init_pos_default = initial_positions_file_choices + else: + init_pos_default, init_pos_all_zeros = initial_positions_file_choices + + possible_combinations = [] + for description_package in description_package_choices: + package_prefix = description_package.split("_")[1] + for description_macro_file in description_macro_file_choices: + char_after_prefix = description_macro_file[len(package_prefix)] + if ( + description_macro_file.startswith(package_prefix) + and not char_after_prefix.isnumeric() + ): + robot_name = description_macro_file.split("_macro")[0] + possible_combinations.append( + KukaArgCombination( + description_package=description_package, + description_macro_file=description_macro_file, + robot_name=robot_name, + controllers_file=controllers_file_7dof + if robot_name in robot_names_7dof + else controllers_file_6dof, + init_pos_file=init_pos_all_zeros + if robot_name in robot_names_init_pos_all_zeros + else init_pos_default, + ) + ) + return possible_combinations + + +### For test debugging ### +test_only_these_robot_names = None # e.g. ["lbr_iiwa_14_r820"] +start_rviz = False +### For test debugging ### + +package_name = "kuka_ros2_control_support" +launch_file_name = "test_bringup.launch.py" +robot_names_7dof = ["lbr_iiwa_14_r820"] +description_package_arg_name = "description_package" +description_macro_file_arg_name = "description_macro_file" +controllers_file_arg_name = "controllers_file" +extra_launch_args = {"use_mock_hardware": "true", "start_rviz": "true" if start_rviz else "false"} +initial_positions_file_arg_name = "initial_positions_file" +robot_names_init_pos_all_zeros = ["kr210l150", "lbr_iiwa_14_r820"] + +possible_combinations = get_possible_combinations( + package_name=package_name, + launch_file_name=launch_file_name, + robot_names_7dof=robot_names_7dof, + description_package_arg_name=description_package_arg_name, + description_macro_file_arg_name=description_macro_file_arg_name, + controllers_file_arg_name=controllers_file_arg_name, + initial_positions_file_arg_name=initial_positions_file_arg_name, + robot_names_init_pos_all_zeros=robot_names_init_pos_all_zeros, +) + +launch_testing_params = [ + ( + f"{description_package_arg_name}={combination.description_package} " + f"{description_macro_file_arg_name}={combination.description_macro_file} " + f"{controllers_file_arg_name}={combination.controllers_file} " + f"{initial_positions_file_arg_name}={combination.init_pos_file}" + ) + for combination in possible_combinations + if not test_only_these_robot_names or combination.robot_name in test_only_these_robot_names +] + +current_combination = -1 + + +@pytest.mark.launch_test +@launch_testing.parametrize("launch_testing_params", launch_testing_params) +def generate_test_description(launch_testing_params): + def parse_args(): + launch_args = {} + for arg in launch_testing_params.split(" "): + key, value = arg.split("=") + launch_args[key] = value + return launch_args + + global current_combination, possible_combinations, package_name, launch_file_name, extra_launch_args + global description_package_arg_name, description_macro_file_arg_name, controllers_file_arg_name + global initial_positions_file_arg_name + current_combination += 1 + + print("-" * 20) + print(f"Testing {current_combination+1}/{len(possible_combinations)} {launch_testing_params}") + print("-" * 20) + + declared_arguments = [] + + launch_args = parse_args() + + robot_driver = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(package_name), "launch", launch_file_name]) + ), + launch_arguments={ + f"{description_package_arg_name}": launch_args[description_package_arg_name], + f"{description_macro_file_arg_name}": launch_args[description_macro_file_arg_name], + f"{controllers_file_arg_name}": launch_args[controllers_file_arg_name], + f"{initial_positions_file_arg_name}": launch_args[initial_positions_file_arg_name], + **extra_launch_args, + }.items(), + ) + + return LaunchDescription(declared_arguments + [ReadyToTest(), robot_driver]) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + + cls.node_name = "ros2_control_support_test" + cls.node = Node(cls.node_name) + cls.executor = rclpy.executors.SingleThreadedExecutor() + cls.executor.add_node(cls.node) + cls.traj_state_topic = "/position_trajectory_controller/state" + cls.joint_trajectory_topic = "/position_trajectory_controller/joint_trajectory" + cls.trajectory_publisher = cls.node.create_publisher( + JointTrajectory, cls.joint_trajectory_topic, 10 + ) + cls.joint_state_topic = "/joint_states" + cls.node.get_logger().info("Setup complete") + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.get_logger().info("Tearing down") + cls.node.destroy_node() + rclpy.shutdown() + + # + # Test functions + # + + def test_node_started(self): + # Verify that the node started successfully + node_names = self.node.get_node_names() + self.assertIn(self.node_name, node_names) + self.node.get_logger().info("Node started successfully") + + def test_joint_state_msg_retrieval(self, time_to_wait=3.0): + ret, _ = wait_for_message( + JointState, + self.node, + self.joint_state_topic, + time_to_wait=time_to_wait, + ) + self.assertTrue( + ret, f"failed to receive a joint state message from topic '{self.joint_state_topic}'" + ) + + def test_trajectory_state_retrieval(self): + ret, _ = self.get_trajectory_state() + self.assertTrue( + ret, + f"failed to receive a trajectory state message from topic '{self.traj_state_topic}'", + ) + + def test_tcp_tf(self): + self.node.get_logger().info("Checking TF for TCP availability and consistency") + ret, tcp_tf = self.get_tcp_transform() + self.assertTrue( + ret, + f"failed to lookup the tcp transform, is robot_state_publisher running ?") + self.assertTrue(tcp_tf.transform.translation != Vector3(), + f"tcp transform translation should not be null") + self.node.get_logger().info("Checking TF for TCP availability and consistency done") + + def test_trajectory_following( + self, position_change=0.2, time_from_start=1, almost_equal_places=2 + ): + def get_joint_states_from_trajectory_state(): + _, msg = self.get_trajectory_state() + return dict(zip(msg.joint_names, msg.actual.positions)) + + self.node.get_logger().info("Waiting for initial joint states") + joint_states = get_joint_states_from_trajectory_state() + + # Create a trajectory based on the initial joint states + self.node.get_logger().info("Creating trajectory") + trajectory = JointTrajectory() + trajectory.joint_names = list(joint_states.keys()) + point = JointTrajectoryPoint() + point.positions = [pos + position_change for pos in joint_states.values()] + point.time_from_start = Duration(sec=time_from_start) + trajectory.points.append(point) + + self.node.get_logger().info(f"Publishing trajectory") + self.trajectory_publisher.publish(trajectory) + time.sleep(2 * time_from_start) + + self.node.get_logger().info("Waiting for joint states after executed trajectory") + joint_states = get_joint_states_from_trajectory_state() + + # Check if the actual joint positions match the target ones + self.node.get_logger().info("Checking joint positions") + for joint_name, target_position in zip(trajectory.joint_names, point.positions): + self.assertAlmostEqual( + joint_states[joint_name], target_position, places=almost_equal_places + ) + self.node.get_logger().info("Checking joint positions is done") + + # + # Utility functions + # + + def get_trajectory_state(self, time_to_wait=3.0): + return wait_for_message( + JointTrajectoryControllerState, + self.node, + self.traj_state_topic, + time_to_wait=time_to_wait, + ) + + def get_tcp_transform(self, time_to_wait=5.0, base_link="base_link", tool_link="tool0"): + tf_buffer = Buffer() + # spinner thread must be true to get the transforms + tf_listener = TransformListener( + buffer=tf_buffer, node=self.node, spin_thread=True) + t = None + try: + ret = tf_buffer.can_transform( + tool_link, + base_link, + rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=time_to_wait)) + if ret: + t = tf_buffer.lookup_transform( + tool_link, + base_link, + rclpy.time.Time()) + except TransformException as ex: + self.node.get_logger().info( + f'Could not transform {base_link} to {tool_link}: {ex}') + + # required to avoid rclpy.executors.ExternalShutdownException + tf_listener.__del__() + if t is not None: + return (True, t) + else: + return (False, None) diff --git a/kuka_ros2_control_support/urdf/common_kuka.xacro b/kuka_ros2_control_support/urdf/common_kuka.xacro new file mode 100644 index 000000000..0a92a8522 --- /dev/null +++ b/kuka_ros2_control_support/urdf/common_kuka.xacro @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kuka_ros2_control_support/urdf/common_macro.ros2_control.xacro b/kuka_ros2_control_support/urdf/common_macro.ros2_control.xacro new file mode 100644 index 000000000..38d1b8b45 --- /dev/null +++ b/kuka_ros2_control_support/urdf/common_macro.ros2_control.xacro @@ -0,0 +1,145 @@ + + + + + + + + + + + + + + + + + mock_components/GenericSystem + + + gazebo_ros2_control/GazeboSystem + + + ign_ros2_control/IgnitionSystem + + + kuka_eki_hw_interface/KukaEkiHardwareInterface + ${eki_robot_ip} + ${eki_robot_port} + + + kuka_rsi_hw_interface/RobotControlInterface + ${rsi_listen_ip} + ${rsi_listen_port} + + + + + + -1 + 1 + + + ${initial_positions['joint_a1']} + + + + + -1 + 1 + + + ${initial_positions['joint_a2']} + + + + + -1 + 0.61 + + + ${initial_positions['joint_a3']} + + + + + -1 + 1 + + + ${initial_positions['joint_a4']} + + + + + -1 + 1 + + + ${initial_positions['joint_a5']} + + + + + -1 + 1 + + + ${initial_positions['joint_a6']} + + + + + + + -1 + 1 + + + ${initial_positions['joint_a7']} + + + + + + + + + + + + ${simulation_controllers} + + + + + + + + + + + ${simulation_controllers} + ${prefix}controller_manager + + + + + + diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index b1afd1987..81e29f29d 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -1,69 +1,74 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_rsi_hw_interface) +cmake_minimum_required(VERSION 3.16) +project(kuka_rsi_hw_interface LANGUAGES CXX) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() -find_package(catkin REQUIRED COMPONENTS - cmake_modules - controller_manager - hardware_interface - joint_limits_interface - realtime_tools - roscpp - std_msgs +option( + KUKA_ROS2_CONTROL_SUPPORT_BUILD_INTEGRATION_TESTS + "Build integration tests" + OFF ) -find_package(Boost REQUIRED COMPONENTS system) -find_package(TinyXML REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + tinyxml_vendor + TinyXML +) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - kuka_hardware_interface - CATKIN_DEPENDS - controller_manager - hardware_interface - joint_limits_interface - roscpp - std_msgs - DEPENDS - TinyXML +add_library(kuka_rsi_hw_interface SHARED + src/robot_control_interface.cpp ) +target_compile_features(kuka_rsi_hw_interface PUBLIC cxx_std_17) +target_include_directories(kuka_rsi_hw_interface PUBLIC + $ + $ +) +ament_target_dependencies(kuka_rsi_hw_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${TinyXML_INCLUDE_DIRS} -) +pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) -add_library(kuka_hardware_interface - src/kuka_hardware_interface.cpp -) -target_link_libraries(kuka_hardware_interface - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${TinyXML_LIBRARIES} -) +if(BUILD_TESTING) + # find_package(launch_testing_ament_cmake) -add_executable(kuka_hardware_interface_node - src/kuka_hardware_interface_node.cpp -) + # if(KUKA_ROS2_CONTROL_SUPPORT_BUILD_INTEGRATION_TESTS) + # add_launch_test(test/test_rsi_hw_interface.py + # TIMEOUT + # 180 + # ) + # endif() +endif() -target_link_libraries(kuka_hardware_interface_node - kuka_hardware_interface +install( + DIRECTORY include/ + DESTINATION include/kuka_rsi_hw_interface ) install( - TARGETS kuka_hardware_interface - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + TARGETS kuka_rsi_hw_interface + EXPORT export_kuka_rsi_hw_interface + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) install( - TARGETS kuka_hardware_interface_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + DIRECTORY config krl + DESTINATION share/kuka_rsi_hw_interface +) + +ament_export_targets(export_kuka_rsi_hw_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/kuka_rsi_hw_interface/config/controller_joint_names.yaml b/kuka_rsi_hw_interface/config/controller_joint_names.yaml deleted file mode 100644 index 5f8f0343a..000000000 --- a/kuka_rsi_hw_interface/config/controller_joint_names.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] diff --git a/kuka_rsi_hw_interface/config/hardware_controllers.yaml b/kuka_rsi_hw_interface/config/hardware_controllers.yaml deleted file mode 100644 index 991bb3c90..000000000 --- a/kuka_rsi_hw_interface/config/hardware_controllers.yaml +++ /dev/null @@ -1,18 +0,0 @@ -#Publish all joint states -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - -# Joint trajectory controller -position_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 - - state_publish_rate: 50 # Defaults to 50 - action_monitor_rate: 20 # Defaults to 20 diff --git a/kuka_rsi_hw_interface/config/hardware_controllers_6dof.yaml b/kuka_rsi_hw_interface/config/hardware_controllers_6dof.yaml new file mode 100644 index 000000000..2797e5adb --- /dev/null +++ b/kuka_rsi_hw_interface/config/hardware_controllers_6dof.yaml @@ -0,0 +1,36 @@ +controller_manager: + ros__parameters: + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +position_trajectory_controller: + ros__parameters: + + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_rsi_hw_interface/config/hardware_controllers_7dof.yaml b/kuka_rsi_hw_interface/config/hardware_controllers_7dof.yaml new file mode 100644 index 000000000..45b5b8783 --- /dev/null +++ b/kuka_rsi_hw_interface/config/hardware_controllers_7dof.yaml @@ -0,0 +1,37 @@ +controller_manager: + ros__parameters: + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +position_trajectory_controller: + ros__parameters: + + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + - joint_a7 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 2.0 # Defaults to 50 + action_monitor_rate: 2.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp similarity index 85% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h rename to kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 9e1f919b3..471c2b78f 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -37,34 +37,29 @@ * Author: Lars Tingelstad */ -#ifndef KUKA_RSI_HARDWARE_INTERFACE_KUKA_HARDWARE_INTERFACE_ -#define KUKA_RSI_HARDWARE_INTERFACE_KUKA_HARDWARE_INTERFACE_ +#ifndef KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ +#define KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ + +// UDP server +#include +#include +#include // STL -#include +#include #include +#include // ROS -#include -#include - +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" // ros_control -#include -#include -#include -#include -#include - -// Timers -#include - -// UDP server -#include - -// RSI -#include -#include +#include +#include +#include +#include +#include namespace kuka_rsi_hw_interface { @@ -74,9 +69,7 @@ static const double DEG2RAD = 0.017453292519943295; class KukaHardwareInterface : public hardware_interface::RobotHW { - private: - // ROS node handle ros::NodeHandle nh_; @@ -98,7 +91,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW std::vector rsi_joint_position_corrections_; unsigned long long ipoc_; - std::unique_ptr > rt_rsi_pub_; + std::unique_ptr> rt_rsi_pub_; std::unique_ptr server_; std::string local_host_; @@ -118,7 +111,6 @@ class KukaHardwareInterface : public hardware_interface::RobotHW hardware_interface::PositionJointInterface position_joint_interface_; public: - KukaHardwareInterface(); ~KukaHardwareInterface(); @@ -126,9 +118,8 @@ class KukaHardwareInterface : public hardware_interface::RobotHW void configure(); bool read(const ros::Time time, const ros::Duration period); bool write(const ros::Time time, const ros::Duration period); - }; -} // namespace kuka_rsi_hw_interface +} // namespace kuka_rsi_hw_interface -#endif +#endif // KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_system_position_only.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_system_position_only.hpp new file mode 100644 index 000000000..8fd3530a9 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_system_position_only.hpp @@ -0,0 +1,93 @@ +#ifndef KUKA_RSI_HW_INTERFACE__KUKA_SYSTEM_POSITION_HPP_ +#define KUKA_RSI_HW_INTERFACE__KUKA_SYSTEM_POSITION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/macros.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "kuka_rsi_hw_interface/visibility_control.hpp" + +#include "kuka_rsi_hw_interface/rsi_command.hpp" +#include "kuka_rsi_hw_interface/rsi_state.hpp" +#include "kuka_rsi_hw_interface/udp_server.hpp" + +using hardware_interface::return_type; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace kuka_rsi_hw_interface +{ +class KukaSystemPositionOnlyHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(KukaSystemPositionOnlyHardware); + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + // return_type configure(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DRIVER_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DRIVER_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + // return_type start() override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + // return_type stop() override; + + ROS2_CONTROL_DRIVER_PUBLIC + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DRIVER_PUBLIC + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Dummy parameters + double hw_start_sec_, hw_stop_sec_, hw_slowdown_; + // Store the command for the simulated robot + std::vector hw_commands_, hw_states_; + + // RSI + kuka_rsi_hw_interface::RSIState rsi_state_; + kuka_rsi_hw_interface::RSICommand rsi_command_; + std::vector rsi_initial_joint_positions_; + std::vector rsi_joint_position_corrections_; + unsigned long long ipoc_; + + std::unique_ptr server_; + std::string local_host_; + int local_port_; + std::string remote_host_; + std::string remote_port_; + std::string in_buffer_; + std::string out_buffer_; + + double loop_hz_; + // double control_period_; + std::chrono::steady_clock::time_point time_now_, time_last_; + std::chrono::duration control_period_, elapsed_time_; + // rclcpp::Duration control_period_; + // rclcpp::Duration elapsed_time_; +}; + +} // namespace kuka_rsi_hw_interface + +#endif // KUKA_RSI_HW_INTERFACE__KUKA_SYSTEM_POSITION_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp new file mode 100644 index 000000000..26d0c76df --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp @@ -0,0 +1,125 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER 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. + *********************************************************************/ + +#ifndef KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_INTERFACE_HPP_ +#define KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/macros.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "kuka_rsi_hw_interface/visibility_control.hpp" + +#include "kuka_rsi_hw_interface/rsi_command.hpp" +#include "kuka_rsi_hw_interface/rsi_state.hpp" +#include "kuka_rsi_hw_interface/udp_server.hpp" + +using hardware_interface::return_type; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace kuka_rsi_hw_interface +{ +class RobotControlInterface : public hardware_interface::SystemInterface +{ +public: + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + // return_type configure(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DRIVER_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DRIVER_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + // return_type start() override; + + ROS2_CONTROL_DRIVER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + // return_type stop() override; + + ROS2_CONTROL_DRIVER_PUBLIC + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DRIVER_PUBLIC + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Dummy parameters + double hw_start_sec_, hw_stop_sec_, hw_slowdown_; + // Store the command for the simulated robot + std::vector hw_commands_, hw_states_; + + // RSI + kuka_rsi_hw_interface::RSIState rsi_state_; + kuka_rsi_hw_interface::RSICommand rsi_command_; + std::vector rsi_initial_joint_positions_; + std::vector rsi_joint_position_corrections_; + unsigned long long ipoc_; + + std::unique_ptr server_; + std::string local_host_; + int local_port_; + std::string remote_host_; + std::string remote_port_; + std::string in_buffer_; + std::string out_buffer_; + + double loop_hz_; + // double control_period_; + std::chrono::steady_clock::time_point time_now_, time_last_; + std::chrono::duration control_period_, elapsed_time_; + // rclcpp::Duration control_period_; + // rclcpp::Duration elapsed_time_; +}; + +} // namespace kuka_rsi_hw_interface + +#endif // KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_INTERFACE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp similarity index 95% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h rename to kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp index c1dea43a6..644b990bf 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp @@ -61,9 +61,9 @@ RSICommand::RSICommand() RSICommand::RSICommand(std::vector joint_position_correction, unsigned long long ipoc) { TiXmlDocument doc; - TiXmlElement* root = new TiXmlElement("Sen"); + TiXmlElement * root = new TiXmlElement("Sen"); root->SetAttribute("Type", "ImFree"); - TiXmlElement* el = new TiXmlElement("AK"); + TiXmlElement * el = new TiXmlElement("AK"); // Add string attribute el->SetAttribute("A1", std::to_string(joint_position_correction[0])); el->SetAttribute("A2", std::to_string(joint_position_correction[1])); @@ -84,6 +84,6 @@ RSICommand::RSICommand(std::vector joint_position_correction, unsigned l xml_doc = printer.Str(); } -} // namespace kuka_rsi_hw_interface +} // namespace kuka_rsi_hw_interface #endif diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp similarity index 89% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h rename to kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp index 3396543e7..73c3150bd 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp @@ -39,21 +39,20 @@ #ifndef KUKA_RSI_HW_INTERFACE_RSI_STATE_ #define KUKA_RSI_HW_INTERFACE_RSI_STATE_ -#include #include +#include namespace kuka_rsi_hw_interface { class RSIState { - private: std::string xml_doc_; public: - RSIState() : - positions(6, 0.0), + RSIState() + : positions(6, 0.0), initial_positions(6, 0.0), cart_position(6, 0.0), initial_cart_position(6, 0.0) @@ -72,11 +71,10 @@ class RSIState std::vector initial_cart_position; // IPOC unsigned long long ipoc; - }; -RSIState::RSIState(std::string xml_doc) : - xml_doc_(xml_doc), +RSIState::RSIState(std::string xml_doc) +: xml_doc_(xml_doc), positions(6, 0.0), initial_positions(6, 0.0), cart_position(6, 0.0), @@ -86,9 +84,9 @@ RSIState::RSIState(std::string xml_doc) : TiXmlDocument bufferdoc; bufferdoc.Parse(xml_doc_.c_str()); // Get the Rob node: - TiXmlElement* rob = bufferdoc.FirstChildElement("Rob"); + TiXmlElement * rob = bufferdoc.FirstChildElement("Rob"); // Extract axis specific actual position - TiXmlElement* AIPos_el = rob->FirstChildElement("AIPos"); + TiXmlElement * AIPos_el = rob->FirstChildElement("AIPos"); AIPos_el->Attribute("A1", &positions[0]); AIPos_el->Attribute("A2", &positions[1]); AIPos_el->Attribute("A3", &positions[2]); @@ -96,7 +94,7 @@ RSIState::RSIState(std::string xml_doc) : AIPos_el->Attribute("A5", &positions[4]); AIPos_el->Attribute("A6", &positions[5]); // Extract axis specific setpoint position - TiXmlElement* ASPos_el = rob->FirstChildElement("ASPos"); + TiXmlElement * ASPos_el = rob->FirstChildElement("ASPos"); ASPos_el->Attribute("A1", &initial_positions[0]); ASPos_el->Attribute("A2", &initial_positions[1]); ASPos_el->Attribute("A3", &initial_positions[2]); @@ -104,7 +102,7 @@ RSIState::RSIState(std::string xml_doc) : ASPos_el->Attribute("A5", &initial_positions[4]); ASPos_el->Attribute("A6", &initial_positions[5]); // Extract cartesian actual position - TiXmlElement* RIst_el = rob->FirstChildElement("RIst"); + TiXmlElement * RIst_el = rob->FirstChildElement("RIst"); RIst_el->Attribute("X", &cart_position[0]); RIst_el->Attribute("Y", &cart_position[1]); RIst_el->Attribute("Z", &cart_position[2]); @@ -112,7 +110,7 @@ RSIState::RSIState(std::string xml_doc) : RIst_el->Attribute("B", &cart_position[4]); RIst_el->Attribute("C", &cart_position[5]); // Extract cartesian actual position - TiXmlElement* RSol_el = rob->FirstChildElement("RSol"); + TiXmlElement * RSol_el = rob->FirstChildElement("RSol"); RSol_el->Attribute("X", &initial_cart_position[0]); RSol_el->Attribute("Y", &initial_cart_position[1]); RSol_el->Attribute("Z", &initial_cart_position[2]); @@ -120,10 +118,10 @@ RSIState::RSIState(std::string xml_doc) : RSol_el->Attribute("B", &initial_cart_position[4]); RSol_el->Attribute("C", &initial_cart_position[5]); // Get the IPOC timestamp - TiXmlElement* ipoc_el = rob->FirstChildElement("IPOC"); + TiXmlElement * ipoc_el = rob->FirstChildElement("IPOC"); ipoc = std::stoull(ipoc_el->FirstChild()->Value()); } -} // namespace kuka_rsi_hw_interface +} // namespace kuka_rsi_hw_interface #endif diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp similarity index 84% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h rename to kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp index 704ae09d0..8541cf068 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp @@ -41,53 +41,55 @@ #define KUKA_RSI_HARDWARE_INTERFACE_UDP_SERVER_ #include -#include #include +#include // Select includes #include -#include +#include +#include +#include #include -#include #include -#include -#include +#include #include -#include -#include +#include +#include #define BUFSIZE 1024 class UDPServer { public: - UDPServer(std::string host, unsigned short port) : local_host_(host), local_port_(port), timeout_(false) + UDPServer(std::string host, unsigned short port) + : local_host_(host), local_port_(port), timeout_(false) { sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd_ < 0) + { throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); + } optval = 1; - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval , sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval, sizeof(int)); memset(&serveraddr_, 0, sizeof(serveraddr_)); serveraddr_.sin_family = AF_INET; serveraddr_.sin_addr.s_addr = inet_addr(local_host_.c_str()); serveraddr_.sin_port = htons(local_port_); - if (bind(sockfd_, (struct sockaddr *) &serveraddr_, sizeof(serveraddr_)) < 0) + if (bind(sockfd_, (struct sockaddr *)&serveraddr_, sizeof(serveraddr_)) < 0) + { throw std::runtime_error("Error binding socket: " + std::string(strerror(errno))); + } clientlen_ = sizeof(clientaddr_); } - ~UDPServer() - { - close(sockfd_); - } + ~UDPServer() { close(sockfd_); } bool set_timeout(int millisecs) { if (millisecs != 0) { - tv_.tv_sec = millisecs / 1000; + tv_.tv_sec = millisecs / 1000; tv_.tv_usec = (millisecs % 1000) * 1000; timeout_ = true; return timeout_; @@ -98,10 +100,11 @@ class UDPServer } } - ssize_t send(std::string& buffer) + ssize_t send(std::string & buffer) { ssize_t bytes = 0; - bytes = sendto(sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); + bytes = sendto( + sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *)&clientaddr_, clientlen_); if (bytes < 0) { std::cout << "ERROR in sendto" << std::endl; @@ -110,7 +113,7 @@ class UDPServer return bytes; } - ssize_t recv(std::string& buffer) + ssize_t recv(std::string & buffer) { ssize_t bytes = 0; @@ -124,7 +127,7 @@ class UDPServer tv.tv_sec = tv_.tv_sec; tv.tv_usec = tv_.tv_usec; - if (select(sockfd_+1, &read_fds, NULL, NULL, &tv) < 0) + if (select(sockfd_ + 1, &read_fds, NULL, NULL, &tv) < 0) { return 0; } @@ -132,7 +135,8 @@ class UDPServer if (FD_ISSET(sockfd_, &read_fds)) { memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + bytes = + recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); if (bytes < 0) { std::cout << "ERROR in recvfrom" << std::endl; @@ -142,19 +146,18 @@ class UDPServer { return 0; } - } else { memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); if (bytes < 0) { std::cout << "ERROR in recvfrom" << std::endl; } } - buffer = std::string(buffer_); + buffer = std::string(buffer_); //this probably allocates, use string.assign()? return bytes; } @@ -171,8 +174,6 @@ class UDPServer struct sockaddr_in clientaddr_; char buffer_[BUFSIZE]; int optval; - }; #endif - diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp new file mode 100644 index 000000000..b32e116bb --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER 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. + *********************************************************************/ + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DRIVER_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DRIVER_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DRIVER_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DRIVER_BUILDING_DLL +#define ROS2_CONTROL_DRIVER_PUBLIC ROS2_CONTROL_DRIVER_EXPORT +#else +#define ROS2_CONTROL_DRIVER_PUBLIC ROS2_CONTROL_DRIVER_IMPORT +#endif +#define ROS2_CONTROL_DRIVER_PUBLIC_TYPE ROS2_CONTROL_DRIVER_PUBLIC +#define ROS2_CONTROL_DRIVER_LOCAL +#else +#define ROS2_CONTROL_DRIVER_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DRIVER_PUBLIC +#define ROS2_CONTROL_DRIVER_LOCAL +#endif +#define ROS2_CONTROL_DRIVER_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src index c8d384875..612e040f9 100644 --- a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src @@ -25,7 +25,7 @@ DEF ros_rsi( ) ; Technology, nor the names of its contributors may be used to ; endorse or promote products derived from this software without ; specific prior written permission. -; +; ; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT ; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -54,7 +54,7 @@ DEF ros_rsi( ) ;FOLD INI ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) - INTERRUPT ON 3 + INTERRUPT ON 3 BAS (#INITMOV,0 ) ;ENDFOLD (BASISTECH INI) ;FOLD USER INI @@ -154,10 +154,10 @@ IF (err <> #RSIOK) THEN HALT ENDIF -; Runs untill RSI break +; Runs until RSI break ST_SKIPSENS() -; Turn off RSI +; Turn off RSI err = ST_OFF() IF (err <> #RSIOK) THEN HALT diff --git a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml index 531e44926..35df8dd28 100644 --- a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml @@ -5,7 +5,7 @@ UDP ImFree FALSE - + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README.md b/kuka_rsi_hw_interface/krl/KR_C4/README.md index 206454eb4..b1084bebf 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/README.md +++ b/kuka_rsi_hw_interface/krl/KR_C4/README.md @@ -112,4 +112,3 @@ $ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller Choose **controller manager ns** and **controller** and you should be able to move each robot joint. * Note that T1-mode limits the robot movement velocity and is intended for testing purposes. - diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi index 25bcf9840..fd00db4e3 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi @@ -1,4 +1,4 @@ - + @@ -109,4 +109,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram index 6386e4208..f42f47b99 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram @@ -1,4 +1,4 @@ - + @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml index 30f5715c6..ab64bd15b 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml @@ -1,4 +1,4 @@ - + @@ -48,4 +48,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src index 9df9f6d6e..8dedecfcb 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src @@ -25,7 +25,7 @@ DEF RSI_AxisCorr( ) ; Technology, nor the names of its contributors may be used to ; endorse or promote products derived from this software without ; specific prior written permission. -; +; ; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT ; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -42,9 +42,9 @@ DEF RSI_AxisCorr( ) ; ============================================= ; ============================================= -; +; ; ROS INDUSTRIAL: KUKA RSI HW INTERFACE -; Realtime UDP data exchange with +; Realtime UDP data exchange with ; kuka_rsi_hw_interface ; ; ============================================= @@ -56,7 +56,7 @@ DECL INT CONTID ; ContainerID ;FOLD INI ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) - INTERRUPT ON 3 + INTERRUPT ON 3 BAS (#INITMOV,0 ) ;ENDFOLD (BASISTECH INI) ;FOLD USER INI @@ -68,7 +68,7 @@ DECL INT CONTID ; ContainerID ; Move to start position PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0} -; Create RSI Context +; Create RSI Context ret = RSI_CREATE("ros_rsi.rsi",CONTID,TRUE) IF (ret <> RSIOK) THEN HALT @@ -83,7 +83,7 @@ ENDIF ; Sensor guided movement RSI_MOVECORR() -; Turn off RSI +; Turn off RSI ret = RSI_OFF() IF (ret <> RSIOK) THEN HALT diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml index 806e89a90..3bdadd954 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml @@ -1,17 +1,17 @@ ip.address.of.rospc - 49152 - ImFree + 49152 + ImFree FALSE - - + + - + diff --git a/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml new file mode 100644 index 000000000..5a9b557aa --- /dev/null +++ b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml @@ -0,0 +1,9 @@ + + + + Driver-plugin for ros2_control using RSI communication to KUKA robots. + + + diff --git a/kuka_rsi_hw_interface/launch/controller_spawner.launch b/kuka_rsi_hw_interface/launch/controller_spawner.launch deleted file mode 100644 index bd10aabdc..000000000 --- a/kuka_rsi_hw_interface/launch/controller_spawner.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index 1e6547c56..755ff7672 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -1,10 +1,10 @@ - + kuka_rsi_hw_interface 0.1.0 - A ROS-Control hardware interface for use with KUKA RSI + A ros2_control hardware interface for use with KUKA RSI communication protocol. Lars Tingelstad - Lars Tingelstad + Dr. Denis Štogl G.A. vd. Hoorn (TU Delft Robotics Institute) BSD @@ -12,14 +12,14 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - cmake_modules - - controller_manager hardware_interface - joint_limits_interface - realtime_tools - roscpp - std_msgs + pluginlib + rclcpp + tinyxml_vendor + + + ament_cmake + diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index f03f13bb6..705e70fd9 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -37,18 +37,25 @@ * Author: Lars Tingelstad */ -#include +#include #include - namespace kuka_rsi_hw_interface { -KukaHardwareInterface::KukaHardwareInterface() : - joint_position_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0), joint_position_command_(6, 0.0), joint_velocity_command_( - 6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6), rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_( - 6, 0.0), ipoc_(0), n_dof_(6) +KukaHardwareInterface::KukaHardwareInterface() +: joint_position_(6, 0.0), + joint_velocity_(6, 0.0), + joint_effort_(6, 0.0), + joint_position_command_(6, 0.0), + joint_velocity_command_(6, 0.0), + joint_effort_command_(6, 0.0), + joint_names_(6), + rsi_initial_joint_positions_(6, 0.0), + rsi_joint_position_corrections_(6, 0.0), + ipoc_(0), + n_dof_(6) { in_buffer_.resize(1024); out_buffer_.resize(1024); @@ -57,9 +64,11 @@ KukaHardwareInterface::KukaHardwareInterface() : if (!nh_.getParam("controller_joint_names", joint_names_)) { - ROS_ERROR("Cannot find required parameter 'controller_joint_names' " + ROS_ERROR( + "Cannot find required parameter 'controller_joint_names' " "on the parameter server."); - throw std::runtime_error("Cannot find required parameter " + throw std::runtime_error( + "Cannot find required parameter " "'controller_joint_names' on the parameter server."); } @@ -67,14 +76,12 @@ KukaHardwareInterface::KukaHardwareInterface() : for (std::size_t i = 0; i < n_dof_; ++i) { // Create joint state interface for all joints - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + joint_state_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); // Create joint position control interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + position_joint_interface_.registerHandle(hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); } // Register interfaces @@ -84,10 +91,7 @@ KukaHardwareInterface::KukaHardwareInterface() : ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded kuka_rsi_hardware_interface"); } -KukaHardwareInterface::~KukaHardwareInterface() -{ - -} +KukaHardwareInterface::~KukaHardwareInterface() {} bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration period) { @@ -98,7 +102,8 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio return false; } - if (rt_rsi_pub_->trylock()){ + if (rt_rsi_pub_->trylock()) + { rt_rsi_pub_->msg_.data = in_buffer_; rt_rsi_pub_->unlockAndPublish(); } @@ -119,7 +124,8 @@ bool KukaHardwareInterface::write(const ros::Time time, const ros::Duration peri for (std::size_t i = 0; i < n_dof_; ++i) { - rsi_joint_position_corrections_[i] = (RAD2DEG * joint_position_command_[i]) - rsi_initial_joint_positions_[i]; + rsi_joint_position_corrections_[i] = + (RAD2DEG * joint_position_command_[i]) - rsi_initial_joint_positions_[i]; } out_buffer_ = RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; @@ -156,7 +162,6 @@ void KukaHardwareInterface::start() // Set receive timeout to 1 second server_->set_timeout(1000); ROS_INFO_STREAM_NAMED("kuka_hardware_interface", "Got connection from robot"); - } void KukaHardwareInterface::configure() @@ -166,17 +171,20 @@ void KukaHardwareInterface::configure() if (nh_.getParam(param_addr, local_host_) && nh_.getParam(param_port, local_port_)) { - ROS_INFO_STREAM_NAMED("kuka_hardware_interface", - "Setting up RSI server on: (" << local_host_ << ", " << local_port_ << ")"); + ROS_INFO_STREAM_NAMED( + "kuka_hardware_interface", + "Setting up RSI server on: (" << local_host_ << ", " << local_port_ << ")"); } else { - std::string msg = "Failed to get RSI listen address or listen port from" - " parameter server (looking for '" + param_addr + "' and '" + param_port + "')"; + std::string msg = + "Failed to get RSI listen address or listen port from" + " parameter server (looking for '" + + param_addr + "' and '" + param_port + "')"; ROS_ERROR_STREAM(msg); throw std::runtime_error(msg); } rt_rsi_pub_.reset(new realtime_tools::RealtimePublisher(nh_, "rsi_xml_doc", 3)); } -} // namespace kuka_rsi_hardware_interface +} // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp deleted file mode 100644 index ba587e348..000000000 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Norwegian University of Science and - * Technology, nor the names of its contributors may be used to - * endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Lars Tingelstad - */ - -#include - -int main(int argc, char** argv) -{ - ROS_INFO_STREAM_NAMED("hardware_interface", "Starting hardware interface..."); - - ros::init(argc, argv, "kuka_rsi_hardware_interface"); - - ros::AsyncSpinner spinner(2); - spinner.start(); - - ros::NodeHandle nh; - - kuka_rsi_hw_interface::KukaHardwareInterface kuka_rsi_hw_interface; - kuka_rsi_hw_interface.configure(); - - // Set up timers - ros::Time timestamp; - ros::Duration period; - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - - controller_manager::ControllerManager controller_manager(&kuka_rsi_hw_interface, nh); - - kuka_rsi_hw_interface.start(); - - // Get current time and elapsed time since last read - timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; - - // Run as fast as possible - while (ros::ok()) - //while (!g_quit) - { - // Receive current state from robot - if (!kuka_rsi_hw_interface.read(timestamp, period)) - { - ROS_FATAL_NAMED("kuka_hardware_interface", "Failed to read state from robot. Shutting down!"); - ros::shutdown(); - } - - // Get current time and elapsed time since last read - timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; - - // Update the controllers - controller_manager.update(timestamp, period); - - // Send new setpoint to robot - kuka_rsi_hw_interface.write(timestamp, period); - } - - spinner.stop(); - ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down."); - - return 0; - -} diff --git a/kuka_rsi_hw_interface/src/kuka_system_position_only.cpp b/kuka_rsi_hw_interface/src/kuka_system_position_only.cpp new file mode 100644 index 000000000..625d97fed --- /dev/null +++ b/kuka_rsi_hw_interface/src/kuka_system_position_only.cpp @@ -0,0 +1,260 @@ +// Copyright 2020 ROS2-Control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "kuka_rsi_hw_interface/kuka_system_position_only.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace kuka_rsi_hw_interface +{ + +// return_type KukaSystemPositionOnlyHardware::configure(const hardware_interface::HardwareInfo &info) +CallbackReturn KukaSystemPositionOnlyHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "on_init()"); + + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), + "expecting exactly 1 command interface"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), + "expecting only POSITION command interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), + "expecting exactly 1 state interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), + "expecting only POSITION state interface"); + return CallbackReturn::ERROR; + } + } + + // RSI + in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 + out_buffer_.resize(1024); + remote_host_.resize(1024); + remote_port_.resize(1024); + + rsi_initial_joint_positions_.resize(6, 0.0); + rsi_joint_position_corrections_.resize(6, 0.0), ipoc_ = 0; + + local_host_ = info_.hardware_parameters["listen_address"]; + local_port_ = stoi(info_.hardware_parameters["listen_port"]); + + RCLCPP_DEBUG( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "robot location: %s:%d", + local_host_.c_str(), local_port_); + + // done + // status_ = hardware_interface::status::CONFIGURED; + // return return_type::OK; + return CallbackReturn::SUCCESS; +} + +CallbackReturn KukaSystemPositionOnlyHardware::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "on_configure()"); + + // just in case - not 100% sure this is the right thing to do . . . + for (size_t i = 0; i < hw_states_.size(); ++i) + { + hw_states_[i] = std::numeric_limits::quiet_NaN(); + hw_commands_[i] = std::numeric_limits::quiet_NaN(); + rsi_initial_joint_positions_[i] = 0.0; + rsi_joint_position_corrections_[i] = 0.0; + } + + return CallbackReturn::SUCCESS; +} + +std::vector +KukaSystemPositionOnlyHardware::export_state_interfaces() +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "export_state_interfaces()"); + + std::vector state_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + return state_interfaces; +} + +std::vector +KukaSystemPositionOnlyHardware::export_command_interfaces() +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "export_command_interfaces()"); + + std::vector command_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + return command_interfaces; +} + +// return_type KukaSystemPositionOnlyHardware::start() // QUESTION: should this be in configure? +CallbackReturn KukaSystemPositionOnlyHardware::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "on_activate()"); + + // Wait for connection from robot + server_.reset(new UDPServer(local_host_, local_port_)); + + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "Connecting to robot . . ."); + + int bytes = server_->recv(in_buffer_); + + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "got some bytes"); + + // Drop empty frame with RSI <= 2.3 + if (bytes < 100) + { + bytes = server_->recv(in_buffer_); + } + if (bytes < 100) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "not enough data received"); + return CallbackReturn::ERROR; + } + + rsi_state_ = kuka_rsi_hw_interface::RSIState(in_buffer_); + + for (size_t i = 0; i < hw_states_.size(); ++i) + { + hw_states_[i] = rsi_state_.positions[i] * 3.14159 / 180.0; + hw_commands_[i] = hw_states_[i]; + rsi_initial_joint_positions_[i] = rsi_state_.initial_positions[i]; + } + ipoc_ = rsi_state_.ipoc; + + out_buffer_ = kuka_rsi_hw_interface::RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; + server_->send(out_buffer_); + server_->set_timeout(1000); // Set receive timeout to 1 second + + RCLCPP_DEBUG( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "System Successfully started!"); + + // status_ = hardware_interface::status::STARTED; + // return return_type::OK; + return CallbackReturn::SUCCESS; +} + +// return_type KukaSystemPositionOnlyHardware::stop() +CallbackReturn KukaSystemPositionOnlyHardware::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "on_deactivate()"); + + RCLCPP_DEBUG( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "System successfully stopped!"); + + return CallbackReturn::SUCCESS; +} + +// WARN: NOT REAL TIME SAFE due to strings/possible allocations +return_type KukaSystemPositionOnlyHardware::read( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "read()"); + + in_buffer_.resize(1024); // FIXME: + if (server_->recv(in_buffer_) == 0) + { // FIXME: server_->recv is probably doing some allocation + return return_type::ERROR; + } + + rsi_state_ = kuka_rsi_hw_interface::RSIState(in_buffer_); + + for (size_t i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = rsi_state_.positions[i] * 3.14159 / 180.0; + RCLCPP_DEBUG( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "Got state %.5f for joint %ld!", + hw_states_[i], i); + } + ipoc_ = rsi_state_.ipoc; + + return return_type::OK; +} + +return_type KukaSystemPositionOnlyHardware::write( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + RCLCPP_DEBUG(rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "write()"); + + out_buffer_.resize(1024); // FIXME + + for (size_t i = 0; i < hw_commands_.size(); i++) + { + RCLCPP_DEBUG( + rclcpp::get_logger("KukaSystemPositionOnlyHardware"), "Got command %.5f for joint %ld!", + hw_commands_[i], i); + rsi_joint_position_corrections_[i] = + (hw_commands_[i] * 180.0 / 3.14159) - rsi_initial_joint_positions_[i]; + } + out_buffer_ = kuka_rsi_hw_interface::RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; + server_->send(out_buffer_); + + return return_type::OK; +} + +} // namespace kuka_rsi_hw_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + kuka_rsi_hw_interface::KukaSystemPositionOnlyHardware, hardware_interface::SystemInterface) diff --git a/kuka_rsi_hw_interface/src/robot_control_interface.cpp b/kuka_rsi_hw_interface/src/robot_control_interface.cpp new file mode 100644 index 000000000..eaec1e04f --- /dev/null +++ b/kuka_rsi_hw_interface/src/robot_control_interface.cpp @@ -0,0 +1,236 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER 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. + *********************************************************************/ + +#include "kuka_rsi_hw_interface/robot_control_interface.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace kuka_rsi_hw_interface +{ + +CallbackReturn RobotControlInterface::on_init(const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger(info_.name), "expecting exactly 1 command interface"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger(info_.name), "expecting only POSITION command interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger(info_.name), "expecting exactly 1 state interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger(info_.name), "expecting only POSITION state interface"); + return CallbackReturn::ERROR; + } + } + + // RSI + in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 + out_buffer_.resize(1024); + remote_host_.resize(1024); + remote_port_.resize(1024); + + rsi_initial_joint_positions_.resize(6, 0.0); + rsi_joint_position_corrections_.resize(6, 0.0), ipoc_ = 0; + + local_host_ = info_.hardware_parameters["listen_address"]; + local_port_ = stoi(info_.hardware_parameters["listen_port"]); + + RCLCPP_INFO( + rclcpp::get_logger(info_.name), "robot location: %s:%d", local_host_.c_str(), local_port_); + + // done + // status_ = hardware_interface::status::CONFIGURED; + // return return_type::OK; + return CallbackReturn::SUCCESS; +} + +CallbackReturn RobotControlInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Wait for connection from robot + server_.reset(new UDPServer(local_host_, local_port_)); + + RCLCPP_INFO(rclcpp::get_logger(info_.name), "Connecting to robot . . ."); + + int bytes = server_->recv(in_buffer_); + + // Drop empty frame with RSI <= 2.3 + if (bytes < 100) + { + bytes = server_->recv(in_buffer_); + } + if (bytes < 100) + { + RCLCPP_FATAL(rclcpp::get_logger(info_.name), "Connection has failed: Not enough data received!"); + return CallbackReturn::ERROR; + } + + rsi_state_ = kuka_rsi_hw_interface::RSIState(in_buffer_); + + for (size_t i = 0; i < hw_states_.size(); ++i) + { + hw_states_[i] = rsi_state_.positions[i] * 3.14159 / 180.0; + hw_commands_[i] = hw_states_[i]; + rsi_initial_joint_positions_[i] = rsi_state_.initial_positions[i]; + } + ipoc_ = rsi_state_.ipoc; + + out_buffer_ = kuka_rsi_hw_interface::RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; + server_->send(out_buffer_); + server_->set_timeout(1000); // Set receive timeout to 1 second + + RCLCPP_INFO(rclcpp::get_logger(info_.name), "Successfully connected with the robot!"); + + return CallbackReturn::SUCCESS; +} + +std::vector RobotControlInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + return state_interfaces; +} + +std::vector RobotControlInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + return command_interfaces; +} + +CallbackReturn RobotControlInterface::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (size_t i = 0; i < hw_states_.size(); ++i) + { + hw_commands_[i] = hw_states_[i]; + } + + return CallbackReturn::SUCCESS; +} + +// return_type RobotControlInterface::stop() +CallbackReturn RobotControlInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(info_.name), "on_deactivate()"); + + RCLCPP_DEBUG(rclcpp::get_logger(info_.name), "System successfully stopped!"); + + return CallbackReturn::SUCCESS; +} + +// WARN: NOT REAL TIME SAFE due to strings/possible allocations +return_type RobotControlInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (server_->recv(in_buffer_) == 0) + { // FIXME: server_->recv is probably doing some allocation + RCLCPP_ERROR(rclcpp::get_logger(info_.name), "Got 0 bytes from server"); + return return_type::ERROR; + } + + rsi_state_ = kuka_rsi_hw_interface::RSIState(in_buffer_); + + for (size_t i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = rsi_state_.positions[i] * 3.14159 / 180.0; + RCLCPP_DEBUG(rclcpp::get_logger(info_.name), "Got state %.5f for joint %ld!", hw_states_[i], i); + } + ipoc_ = rsi_state_.ipoc; + + return return_type::OK; +} + +return_type RobotControlInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (size_t i = 0; i < hw_commands_.size(); i++) + { + RCLCPP_DEBUG( + rclcpp::get_logger(info_.name), "Got command %.5f for joint %ld!", hw_commands_[i], i); + rsi_joint_position_corrections_[i] = + (hw_commands_[i] * 180.0 / 3.14159) - rsi_initial_joint_positions_[i]; + } + out_buffer_ = kuka_rsi_hw_interface::RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; + server_->send(out_buffer_); + + return return_type::OK; +} + +} // namespace kuka_rsi_hw_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + kuka_rsi_hw_interface::RobotControlInterface, hardware_interface::SystemInterface) diff --git a/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml b/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml index ba5e3067b..44f2d28a9 100644 --- a/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml +++ b/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml @@ -15,5 +15,3 @@ position_trajectory_controller: - ag1_joint_6 state_publish_rate: 50 # Defaults to 50 action_monitor_rate: 20 # Defaults to 20 - - diff --git a/kuka_rsi_hw_interface/test/ag2_params.yaml b/kuka_rsi_hw_interface/test/ag2_params.yaml index fbb11e86e..21869f3b4 100644 --- a/kuka_rsi_hw_interface/test/ag2_params.yaml +++ b/kuka_rsi_hw_interface/test/ag2_params.yaml @@ -1,4 +1,3 @@ rsi: listen_address: "192.168.1.11" listen_port: 49152 - diff --git a/kuka_rsi_simulator/CMakeLists.txt b/kuka_rsi_simulator/CMakeLists.txt index 29c1aea04..429fdfbca 100644 --- a/kuka_rsi_simulator/CMakeLists.txt +++ b/kuka_rsi_simulator/CMakeLists.txt @@ -1,13 +1,14 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kuka_rsi_simulator) +cmake_minimum_required(VERSION 3.16) +project(kuka_rsi_simulator LANGUAGES CXX) -find_package(catkin REQUIRED) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) -install( - PROGRAMS scripts/kuka_rsi_simulator - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_python_install_package(kuka_rsi_simulator + SCRIPTS_DESTINATION lib/kuka_rsi_simulator +) +ament_package() diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py b/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py new file mode 100755 index 000000000..3fabd9edc --- /dev/null +++ b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +import argparse +import sys +import socket +import numpy as np +import xml.etree.ElementTree as ET + + +import errno +import rclpy +from std_msgs.msg import String + +def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc): + q = act_joint_pos + qd = setpoint_joint_pos + root = ET.Element('Rob', {'TYPE':'KUKA'}) + ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', + 'A':'0.0', 'B':'0.0', 'C':'0.0'}) + ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', + 'A':'0.0', 'B':'0.0', 'C':'0.0'}) + ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]), + 'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])}) + ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]), + 'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])}) + ET.SubElement(root, 'Delay', {'D':str(timeout_count)}) + ET.SubElement(root, 'IPOC').text=str(ipoc) + return ET.tostring(root) + +def parse_rsi_xml_sen(data): + root = ET.fromstring(data) + AK = root.find('AK').attrib + desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'], + AK['A4'], AK['A5'], AK['A6']]).astype(np.float64) + IPOC = root.find('IPOC').text + return desired_joint_correction, int(IPOC) + + +def main(args=None): + rclpy.init(args=args) + parser = argparse.ArgumentParser(description='KUKA RSI Simulation') + parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)') + parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)') + parser.add_argument('--timeout', default=1.0, type=float, help='Set the timeout of the socket.If zero or negativ number is set, socket will be non-blocking') + + # Only parse known arguments + args, _ = parser.parse_known_args() + host = args.rsi_hw_iface_ip + port = int(args.rsi_hw_iface_port) + timeout = args.timeout + blocking = True + if timeout <= 0.0: + timeout = None + blocking = False + + + # Configuration + node_name = 'kuka_rsi_simulation' + act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64) + cmd_joint_pos = act_joint_pos.copy() + des_joint_correction_absolute = np.zeros(6) + timeout_count = 0 + ipoc = 0 + + node = rclpy.create_node(node_name) + + node.get_logger().info(f"Started '{node_name}' node.") + + rsi_act_pub = node.create_publisher(String, '~/rsi/state', 1) + rsi_cmd_pub = node.create_publisher(String, '~/rsi/command', 1) + + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + # if timout > 0.0 socket is blocking -> using this as for synchronization + s.settimeout(timeout) + node.get_logger().info(f"Successfully created socket with timeout={timeout}s and blocking={blocking}.") + except socket.error as e: + node.get_logger().fatal(f"Could not create socket.") + sys.exit() + + node.get_logger().info(f"Server address set {host}:{port}. This is used to sent data to.") + + while rclpy.ok(): + try: + str_data = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc) + msg = String() + msg.data = str(str_data) + rsi_act_pub.publish(msg) + s.sendto(str_data, (host, port)) + recv_msg, addr = s.recvfrom(1024) + recv_host, recv_port = addr + if recv_host != host or recv_port != port: + node.get_logger().info(f"Server receive data from wrong address/port {host}:{port}.") + continue + msg = String() + msg.data = str(recv_msg) + rsi_cmd_pub.publish(msg) + des_joint_correction_absolute, _ = parse_rsi_xml_sen(recv_msg) + act_joint_pos = cmd_joint_pos + des_joint_correction_absolute + ipoc += 1 + except socket.timeout: + node.get_logger().warn(f"Socket timed out.") + timeout_count += 1 + except socket.error as e: + if e.errno != errno.EINTR: + raise + + node.get_logger().info(f"Shutting down '{node_name}' node.") + node.destroy_node() + rclpy.shutdown() + s.close() diff --git a/kuka_rsi_simulator/launch/kuka_rsi_simulator.launch b/kuka_rsi_simulator/launch/kuka_rsi_simulator.launch deleted file mode 100644 index 7dfdd2bf5..000000000 --- a/kuka_rsi_simulator/launch/kuka_rsi_simulator.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml index e32385ba2..89096f5b7 100644 --- a/kuka_rsi_simulator/package.xml +++ b/kuka_rsi_simulator/package.xml @@ -1,19 +1,24 @@ - + kuka_rsi_simulator 0.1.0 Python node that implements a minimal RSI interface simulator. Lars Tingelstad - Lars Tingelstad G.A. vd. Hoorn (TU Delft Robotics Institute) + Dr. Denis Štogl BSD http://wiki.ros.org/kuka_rsi_simulator https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake + ament_cmake_python - rospy + rclpy std_msgs + + + ament_cmake + diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator deleted file mode 100755 index 15131e167..000000000 --- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python - -import sys -import socket -import numpy as np -import time -import xml.etree.ElementTree as ET - -import errno -import rospy -from std_msgs.msg import String - -def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc): - q = act_joint_pos - qd = setpoint_joint_pos - root = ET.Element('Rob', {'TYPE':'KUKA'}) - ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', - 'A':'0.0', 'B':'0.0', 'C':'0.0'}) - ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', - 'A':'0.0', 'B':'0.0', 'C':'0.0'}) - ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]), - 'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])}) - ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]), - 'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])}) - ET.SubElement(root, 'Delay', {'D':str(timeout_count)}) - ET.SubElement(root, 'IPOC').text=str(ipoc) - return ET.tostring(root) - -def parse_rsi_xml_sen(data): - root = ET.fromstring(data) - AK = root.find('AK').attrib - desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'], - AK['A4'], AK['A5'], AK['A6']]).astype(np.float64) - IPOC = root.find('IPOC').text - return desired_joint_correction, int(IPOC) - -node_name = 'kuka_rsi_simulation' -rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1) -rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1) - -cycle_time = 0.004 -act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64) -cmd_joint_pos = act_joint_pos.copy() -des_joint_correction_absolute = np.zeros(6) -timeout_count = 0 -ipoc = 0 - -if __name__ == '__main__': - import argparse - parser = argparse.ArgumentParser(description='KUKA RSI Simulation') - parser.add_argument('rsi_hw_iface_ip', help='The ip address of the RSI control interface') - parser.add_argument('rsi_hw_iface_port', help='The port of the RSI control interface') - parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. ') - # Only parse known arguments - args, _ = parser.parse_known_args() - host = args.rsi_hw_iface_ip - port = int(args.rsi_hw_iface_port) - sen_type = args.sen - - rospy.init_node(node_name) - rospy.loginfo('{}: Started'.format(node_name)) - - try: - s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - rospy.loginfo('{}, Successfully created socket'.format(node_name)) - s.settimeout(1) - except socket.error as e: - rospy.logfatal('{}Could not create socket'.format(node_name)) - sys.exit() - - def shutdown_hook(): - rospy.loginfo('{}: Shutting down'.format(node_name)) - s.close() - - rospy.on_shutdown(shutdown_hook) - - while not rospy.is_shutdown(): - try: - msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc) - rsi_act_pub.publish(msg) - s.sendto(msg, (host, port)) - recv_msg, addr = s.recvfrom(1024) - rsi_cmd_pub.publish(recv_msg) - des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg) - act_joint_pos = cmd_joint_pos + des_joint_correction_absolute - ipoc += 1 - time.sleep(cycle_time / 2) - except socket.timeout, msg: - rospy.logwarn('{}: Socket timed out'.format(node_name)) - timeout_count += 1 - except socket.error, e: - if e.errno != errno.EINTR: - raise - diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg new file mode 100644 index 000000000..3abc941b4 --- /dev/null +++ b/kuka_rsi_simulator/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +console_scripts = + kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main