diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..10dd2c7c --- /dev/null +++ b/.clang-format @@ -0,0 +1,16 @@ +--- +Language: Cpp +BasedOnStyle: Google + +ColumnLimit: 100 +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BreakBeforeBraces: Allman +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeBlocks: Preserve +InsertBraces: true +... diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml new file mode 100644 index 00000000..32ff262c --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -0,0 +1,80 @@ +name: "Bug Report" +description: Create a new ticket for a bug. +title: "[BUG] - " +labels: [ + "bug" +] +body: + - type: textarea + id: description + attributes: + label: "Description" + description: Please enter an explicit description of your issue + placeholder: Describe your issue in a few sentences + validations: + required: true + - type: dropdown + id: robot-os + attributes: + label: "KUKA robot OS" + description: Version of the KUKA OS are you using + multiple: true + options: + - KSS + - Sunrise + - iiQKA + - type: input + id: os-version + attributes: + label: "KUKA robot OS version" + description: Version of the KUKA robot OS + placeholder: eg. KSS 8.6 + validations: + required: true + - type: input + id: if-version + attributes: + label: "KUKA external interface version" + description: Version of the KUKA external interface + placeholder: eg. RSI 4.1.3 + validations: + required: true + - type: input + id: robot-model + attributes: + label: "Affected robot model(s)" + description: Robot model the issue came up with + placeholder: eg. KR10 R1100-2 + validations: + required: true + - type: input + id: driver-version + attributes: + label: "Version or commit hash of the driver" + description: If the issue came up with an older version (not master), provide the release version or commit hash + validations: + required: false + - type: textarea + id: setup + attributes: + label: "Setup" + description: Describe your setup, launch files and executables started (optionally attach rqt_graph output), modifications to code + render: bash + validations: + required: true + - type: textarea + id: reprod + attributes: + label: "Reproduction steps" + description: Clear and ordered steps of reporoducing + render: bash + validations: + required: true + - type: textarea + id: logs + attributes: + label: "Logs" + description: Attach relevant log output here, mentioning if lines were omitted to help readability. + render: bash + validations: + required: false diff --git a/.github/workflows/deploy_wiki.yml b/.github/workflows/deploy_wiki.yml index 2b96f6a2..7976b57c 100644 --- a/.github/workflows/deploy_wiki.yml +++ b/.github/workflows/deploy_wiki.yml @@ -3,8 +3,9 @@ name: Deploy Wiki on: push: paths: - # Trigger only when wiki directory changes + # Trigger only when wiki directory or pipeline changes - 'doc/wiki/**' + - '.github/workflows/deploy_wiki.yml' branches: # And only on master branch - master @@ -16,11 +17,8 @@ jobs: - uses: actions/checkout@v2 - name: Push Wiki Changes - uses: resizoltan/github-wiki-action@master + uses: SwiftDocOrg/github-wiki-publish-action@v1 + with: + path: "doc/wiki" env: - # Make sure you have that / at the end. We use rsync - # WIKI_DIR's default is wiki/ - WIKI_DIR: doc/wiki/ - GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} - GH_MAIL: resizoltan@gmail.com - GH_NAME: resizoltan + GH_PERSONAL_ACCESS_TOKEN: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }} diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 20a1d32c..242eb8bb 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -55,3 +55,14 @@ jobs: # Run industrial_ci - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} + codespell: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4.7.1 + with: + python-version: '3.10' + - run: pip install git+https://github.com/codespell-project/codespell.git + - uses: codespell-project/actions-codespell@v2 + with: + check_filenames: true diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 00000000..2e1400f7 --- /dev/null +++ b/.github/workflows/pre-commit.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@v4 + - uses: actions/setup-python@v4.7.1 + with: + python-version: '3.10' + - name: Install system hooks + run: sudo apt install -qq cppcheck + - uses: pre-commit/action@v3.0.0 + with: + extra_args: --all-files --hook-stage manual diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..c18dd8d8 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..5631f349 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,140 @@ + +# 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 + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v3.4.0 + hooks: + - id: pyupgrade + args: [--py36-plus] + + # 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/psf/black + rev: 23.3.0 + hooks: + - id: black + args: ["--line-length=99"] + + - repo: https://github.com/pycqa/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + args: ["--extend-ignore=E501"] + + # CPP hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v15.0.6 + hooks: + - id: clang-format + exclude: .*/fri_client_sdk/.*|.*/mock/.*|.*/robot_application/.* + + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + exclude: .*/fri_client_sdk/.*|.*/mock/.* + description: Static code analysis of C/C++ files. + stages: [commit] + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=TRUE 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 + exclude: .*/fri_client_sdk/.*|.*/mock/.* + 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 + exclude: .*/fri_client_sdk/.*|.*/mock/.* + 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.4 + hooks: + - id: codespell + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc)$ diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f651c41c..169fcf32 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,10 +1,34 @@ -The ROS2 KUKA Sunrise driver is being developed and maintained by the kroshu team, but everyone is welcome to contribute. Please take the following aspects into consideration when making contributions: +## Issue tracking -- By default the Git ["Fork and Branch"](https://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/) workflow should be followed. -- To foster a high quality product, feature extensions and improvements shall be done after careful design choices. Therefore, design documents and discussion with the maintainers prior to any development is strongly encouraged to make sure that everyone agrees on the issue and the suggested solution. Depending on the individual case the way to do this could be through a GitHub issue, a pull request to the design drafts folder or a new discussion on the repository's Discussions forum. -- Code quality in this repository is monitored via industrial_ci and SonarCloud. All changes must pass the checks performed by these. (There is some flexibility to test coverage) +If you have questions, suggestions or found a bug, feel free to open an [issue](https://github.com/kroshu/kuka_drivers/issues). +When filing an issue, please check open issues to make sure somebody else hasn't already reported it. Please try to include as much information as you can, including: +- A reproducible test case or series of steps +- The version/commit hash of our code being used +- Any modifications you've made relevant to the bug +- Anything unusual about your environment or deployment +Please do not add labels to the issue, that should be handled by the maintainers. +## Contributing + +Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: + +- Your PR addresses only one issue +- Your PR has a descriptive title and a short summary +- All pipelines are green, including + - Industrial CI + - Sonarcloud + - Spell checks + - Linters + +To send us a pull request, please: +- Discuss the proposed changes with the maintainers, preferably in an issue +- Fork the repository +- Modify the source focusing on the specific change you are contributing +- Ensure that local tests pass (`colcon test` and `pre-commit` run) +- Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation + +## Licensing Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): @@ -18,4 +42,3 @@ be under the Apache 2 License, as dictated by that the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. ~~~ - diff --git a/README.md b/README.md index 5fc60efe..d9fadb8f 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,18 @@ # ROS2 KUKA Drivers -Experimental ROS2 driver for KUKA robots. Recommended distribution is [ROS 2 Humble Hawksbill:](https://docs.ros.org/en/humble/Installation.html). - -> ROS2 Iron Irwini has breaking changes, thus it is not yet supported. +This repository contains ROS2 drivers for all KUKA operating systems. Github CI | SonarCloud ------------| --------------- [![Build Status](https://github.com/kroshu//kuka_drivers/workflows/CI/badge.svg?branch=master)](https://github.com/kroshu/ros2_kuka_sunrise_fri_driver/actions) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=kroshu_kuka_drivers&metric=alert_status)](https://sonarcloud.io/dashboard?id=kroshu_kuka_drivers) +# Requirements +The drivers require a system with ROS installed. It is recommended to use Ubuntu 22.04 with ROS humble. Iron Irwini has breaking changes in the moveit API, thus it is not yet supported. +It is also recommended to use a client machine with a real-time kernel, as all three drivers require cyclic, real-time communication. Due to the real-time requirement, Windows systems are not recommended and covered in the documentation. + + # Installation +The driver is not available as a binary package, building from source is necessary. Create ROS2 workspace (if not already created). ```bash @@ -49,6 +53,7 @@ Source built KUKA packages. source ~/ros2_ws/install/setup.bash ``` -# Get Started +# Getting Started +Documentation of this project can be found on the repository's [Wiki](https://github.com/kroshu/kuka_drivers/wiki) page. -Documentation of this project can be found on the repository's [Wiki](https://github.com/kroshu/kuka_drivers/wiki) page. \ No newline at end of file +If you find something confusing, not working, or would like to contribute, please read our [contributing guide](CONTRIBUTING.md) before opening an issue or creating a pull request. diff --git a/doc/lego_project.drawio b/doc/lego_project.drawio deleted file mode 100644 index efb6996a..00000000 --- a/doc/lego_project.drawio +++ /dev/null @@ -1 +0,0 @@ -<mxfile host="app.diagrams.net" modified="2022-06-01T09:40:43.625Z" agent="5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/101.0.4951.67 Safari/537.36" etag="QTA4iZCYr3pdhk9hODCw" version="18.1.3" type="device" pages="2"><diagram id="Ur5KIAmLowO2MCIeCR-E" name="architectural overview">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</diagram><diagram id="bbBwzUKokB7HsPf8g7Md" name="driver-controller-architecture">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</diagram></mxfile> \ No newline at end of file diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index 6b76fcda..535d960d 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -1,142 +1,117 @@ # Welcome to the ROS2 KUKA drivers project! -## KUKA Sunrise driver (FRI) +## Goals of the project -### Project description +This project aims to provide reliable real-time capable drivers for all KUKA robots. Currently KUKA robots are available with 3 different operating systems with real-time control API-s: +- KSS supporting industrial robots, with Robot Sensor Interface (RSI) +- Sunrise supporting cobots (LBR iiwa-s), with Fast Robot Interface (FRI) +- iiQKA supporting cobots (LBR iisy-s), with ExternalAPI.Control (EAC) -This project centers on the development of a comprehensive ROS2 driver for external control of KUKA robots running on Sunrise OS through the real-time Fast Robot Interface (FRI). The following aspects are given particular consideration: +It is also the goal of this project to provide the same API for all three OS-s, hiding the underlying startup procedure and communication technology, thus enabling changing seamlessly to a different KUKA OS. -1. Application lifecycle controlled completely from ROS2, e.g. - - - Parameter configuration - - - Start/stop of external monitoring and commanding - - - Reacting to application errors +Additionally the aim was to write high quality, maintainable code with standardized interfaces, that conforms with the standards defined by the [ROS-Industrial project](https://www.rosin-project.eu/). -2. Exposing all functionality of the FRI towards the ROS2 system, e.g. - - - All command and control modes of joints - - - Field bus I/O handling +## Common interface -3. Exploiting new features of ROS2 w.r.t. ROS1, e.g. - - - Lifecycle management - - - Node-based ROS parameters - - - Real-time inter-node communication - -### Current state of project - -This project is currently experimental. The functionalities are not fully tested and the API and the internal architecture of the driver should be expected to change. This wiki is targeted at developers who would like to understand and potentially contribute to the project. Usage of this driver is discouraged except for the testing of the driver, only in controlled environment and with the proper safety configuration applied. - -### Features - -The following features of the FRI are exposed to ROS2: +Two different interfaces should be defined for all drivers supporting real-time control: +- real-time interface, defining how to handle the cyclic dataflow +- non-real-time interface, defining the startup procedure with optional configuration #### Real-time interface +The choice for the real-time interface was straightforward, as a standardized control framework exists for ROS2, called `ros2_control`, also supported by ROS-Industrial. The drivers are built using this framework, therefore it is recommended the read through its [documentation](https://control.ros.org/master/doc/ros2_control/doc/index.html), as this documentation builds on the knowledge of the framework. -- [ ] Monitor joint states - - [x] Actual position - - [ ] Actual torque - - [ ] Setpoint position - - [ ] Setpoint torque - - [x] External torque -- [ ] Joint commands - - [x] Position - - [x] Torque -- [ ] Field bus - - [ ] Inputs - - [ ] Outputs -- [ ] Connection quality -- [ ] Safety state -- [x] Tracking performance - -#### Manager interface - -- [x] Session state -- [x] Control mode -- [x] Client command mode -- [ ] Operation mode -- [ ] Number of axes - -## KUKA KSS driver (RSI) - -Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The guide to set up this driver on a real robot can be found in kuka_kss_rsi_driver\krl for both KCR4 and KRC5 controllers. - -### Simulation - -To try out the driver with an open-loop simulation the driver and the kuka_rsi_simulator must be started, (at first only a "collapsed" robot will be visible in rviz): - -**`ros2 launch kuka_kss_rsi_driver startup_with_rviz.launch.py`** - -**`ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py`** - -After all components have started successfully, the system needs to be configured and activated to start the simulation, the robot will be visible in rviz after activation: - -**`ros2 lifecycle set robot_manager configure`** - -**`ros2 lifecycle set robot_manager activate`** - -## iiQKA driver (ECI) - -The third project in the repo is the driver for iiQKA robots. - -### Architecture - -The driver uses the ros2_control framework, so a variaty of controllers are supported and it can be easily integrated into moveit. It consists of a realtime component for controlling the robot via UDP (ROS2 hardware interface) and a non-realtime one for lifecycle management of the controllers and the hardware interface. The driver supports a control cycle time of 4 milliseconds, but the round-trip time of one cycle should not exceed 3 milliseconds, as above that the packets are considered lost. Therefore it is advised to run the driver on a realtime-capable Linux machine (with the PRREMPT_RT patch applied). After a few lost packets the connection is considered not stable enough and external control is ended. - -The driver depends on some KUKA-specific packages, which are only available with the real robot, but setting the MOCK_HW_ONLY flag in the hardware_interface enables the usage of the driver in a simulated way, so that motion planning problems can be tried out with the same components running. -Two additional packages (not listed in package.xml) must be installed with apt: -- libnanopb-dev -- libgrpc++-dev +All 3 of the KUKA real-time interfaces handle the timing on the controller side, so external control is always synchronized with the internal control cycle. This means, that calling the `read` method of the `controller_manager` cannot return immediately, but has to wait until the controller sends an update, which is triggered by the internal clock. Because of this blocking read, the default `ros2_control_node` cannot be used, as there it is expected that the `controller_manager` handles the timing according to the configured control frequency. Therefore a [custom control node](https://github.com/kroshu/kuka_drivers/blob/master/kuka_drivers_core/src/control_node.cpp) was implemented that uses the `controller_manager` and all other tools of `ros2_control`, but leaves the time management to the robot controller. -### Setup +This change does not influence the API of the `ros2_control` framework, the real-time dataflow can be accessed by any controller. -By default, the mock libraries are used, this can be changed in the cmake file by setting MOCK_KUKA_LIBS to FALSE before building. +#### Non-real-time interface +The startup procedure for any system in ROS can be defined using a launch file, that can start multiple processes. By default, starting the control node with a hardware interface and controllers configured immediately starts external control. This behaviour has a few drawbacks: +- The user cannot configure some parameters during runtime, that cannot be changed during external control +- The user cannot easily synchronize the start of external control with other components of the system +- It can cause unexpected behaviour, which can be potentially dangerous to the hardware or surroundings. + - In torque control mode, the robot can start to move, if the torque sensors are not perfectly calibrated (and as that is hard to achieve, this can happen in most cases). This could be mitigated with a simple torque controller that tries to hold the position, but there is no guarantee that loading and activating the controller was successful, external control would start even if it failed. This could result in a scenario, where the robot starts to move unexpectedly. -The IP addresses of the client machine and controller must be given in the *config/driver_config.yaml* configuration file. A rebuild is not needed after the changes, but the file has to be modified before starting the nodes. The control mode of the robot can also be modified in the same configuration file: you can choose either 1 (POSITION_CONTROL), 3 (JOINT_IMPEDANCE_CONTROL) or 5 (TORQUE_CONTROL). This also sets the control_mode parameter of the robot manager node, which can be only modified at startup, control mode changes are not supported in runtime at the current state. +The last issue should be certainly prevented from happening, therefore it was decided to extend the default startup procedure with a [lifecycle interface](https://design.ros2.org/articles/node_lifecycle.html), that synchronizes all components of the driver. The hardware interfaces and controllers already have a lifecycle interface, but by default they are loaded and activated at startup. This configuration was modified to only load the hardware interfaces and controllers, configuration and activation is handled by a custom a lifecycle node, called `robot_manager`. The 3 states of the `robot_manager` node have the following meaning: -Besides, the setting of scheduling priorities must be allowed for your user (extend /etc/security/limits.conf with "username - rtprio 98" and restart) to enable real-time performance. +- `unconfigured`: all necessary components are started, but no connection is needed to the robot +- `configured`: The driver has configured the parameters necessary to start external control. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface. +- `active`: external control is running with cyclic real-time communication, controllers are active -### Usage +To achieve these synchronized states, the state transitions of the system do the following steps (implemented by the launch file and the `robot_manager` node): +- startup: all components of the system are started: `control_node`, `robot_manager` node, `robot_state_publisher` (optionally `rviz`) and all of the necessary controllers are loaded and configured +- `configure`: activate configuration controllers, configure hardware interface +- `activate`: activate real-time controllers, activate hardware interface +- `deactivate`: deactivate hardware interface, deactivate real-time controllers +- `cleanup`: clean up hardware interface, deactivate configuration controllers -The usage of the driver is quite simple, one has to start the launch file in the package to start all required nodes (it also starts an rviz node for visualisation, which can be commented out if not needed): +Note: the lifecycle interface of the controllers are a little bit different, as they do not have a `cleanup` transition. To have a consequent `unconfigured` state, the configuration of the controllers are not handled in the `configure` transition, but at startup. -**`ros2 launch kuka_iiqka_eac_driver startup.launch.py`** +Including the controller state handling in the system state makes the implementation more complex, as controllers must be deactivated and activated at control mode changes, but it has two advantages: + - minor performance increase: unused controllers are not active and therefore do not consume resources + - unexpected behaviour is not possible: external control will not start on the robot, unless all necessary controllers are successfully activated, while control mode changes (on the robot) are only possible after the controllers for the new control mode are activated. -After all components have started successfully, the system needs to be configured and activated to start external control: +The consequence of the lifecycle interface is, that 3 commands are necessary to start external control for all robots: + - start the appropriate launch file for your robot with your robot model as parameter (details can be found [here](#detailed-setup-and-startup-instructions)) + - `ros2 lifecycle set robot_manager configure` + - `ros2 lifecycle set robot_manager activate` -**`ros2 lifecycle set robot_manager configure`** +#### Control mode definitions +The control mode specifications are also part of the common API. They are defined as an enum in the [`kuka_drivers_core`](https://github.com/kroshu/kuka_drivers/blob/master/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp) package, and have the following meaning: -**`ros2 lifecycle set robot_manager activate`** +- joint position control: the driver streams cyclic position updates for every joint. + - Needed command interface(s): `position` +- joint impedance control: the driver streams cyclic position updates for every joint and additionally stiffness [Nm/rad] and normalized damping [-] attributes, which define how the joint reacts to external effects (around the setpoint position). The effect of gravity is compensated internally. + - Needed command interface(s): `position`, `stiffness`, `damping` +- joint velocity control: the driver streams cyclic velocity updates for every joint. + - Needed command interface(s): `velocity` +- joint torque control: the driver streams cyclic torque updates for every joint, which define the torque overlay to be superimposed over gravity compensation. (An input of 0 means, that the joint should remain in gravity compensatin and should not move.) + - Needed command interface(s): `effort` +- cartesian position control: the driver streams cyclic pose updates for every degree of freedom. The orientation representation is the KUKA ABC convention. It is the responsibility of the user to stream poses, for which a valid IK solution exists. + - Needed command interface(s): `cart_position` +- cartesian impedance control: the driver streams cyclic pose updates for every degree of freedom. Additional stiffness [N/m or Nm/rad] and normalized damping [-] attributes define the behaviour of each degree of freedom to external forces. The nullspace stiffness and damping values define the behaviour of the redundant degree(s) of freedom. + - Needed command interface(s): `cart_position`, `cart_stiffness`, `cart_damping`, (`nullspace_stiffness`, `nullspace_damping`) +- cartesian velocity control: the driver streams cyclic cartesian velocity (twist) updates for every degree of freedom. It is the responsibility of the user to stream velocities, for which a valid IK solution exists. + - Needed command interface(s): `cart_velocity` +- wrench control: the driver streams cyclic wrench updates, which define the forces and torques, that the robot end effector should exert on the environment. The effect of gravity is internally compensated. (If the environment does not have a counterforce, the robot will start to move) + - Needed command interface(s): `wrench` -On successful activation the robot controller and the driver start communication with a 4 ms cycle time, and it is possible to move the robot through the joint trajectory controller. The easiest way to achieve this is to start an rqt_joint_trajcectory controller and move the joints with cursors or one can also execute trajectories planned with moveit - an example of this can be found in kuka_sunrise_fri_driver_control/iiqka_moveit_example package. -To stop external control, the components have to be deactivated with **`ros2 lifecycle set robot_manager deactivate`** +#### Supported features +The following table shows the supported features and control modes of each driver. (`✓` means supported, `✗` means not supported by the KUKA interface, empty means supported by the KUKA interface, but not yet supported by the driver) -BEWARE, that this is a non-realtime process including lifecycle management, so the connection is not terminated immediately, in cases where an abrupt stop is needed, the safety stop of the SmartPad should be used! +|OS | Joint position control | Joint impedance control | Joint velocity control | Joint torque control | Cartesian position control | Cartesian impedance control | Cartesian velocity control | Wrench control| I/O control| +|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| +|KSS| ✓ | ✗ | ✗ | ✗ | | ✗ | ✗ | ✗ | | +|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ | | | +|iiQKA| ✓ | ✓ | ✗ | ✓ | ✗ | ✗ | ✗ | ✗ | ✗ | -It is also possible to use different controllers with some modifications in the launch and yaml files (for example ForwardCommandController, which forwards the commands send to a ROS2 topic towards the robot). In these cases, one has to make sure, that the commands sent to the robot are close to the current position, otherwise the machine protection will stop the robot movement. -### Issues +## Additional packages +The repository contains a few other packages aside from the 3 drivers: +- `kuka_driver_interfaces`: this package contains the custom message definition necessary for KUKA robots. +- `kuka_drivers_core`: this package contains core functionalities used by more drivers, including the `control_node`, base classes for nodes with improved parameter handling, enum and constant definitions and a class for managing the controller activation and deactivation at control mode changes. Details about these features can be found in the package [documentation](https://github.com/kroshu/kuka_drivers/blob/master/kuka_drivers_core/README.md) +- `kuka_rsi_simulator`: this package contains a simple simulator of RSI, that implements a UDP server accepting the same xml format as RSI and returning the commanded values as the current state, without any checks. +- `iiqka_moveit_example`: this package contains basic examples of using moveit with the driver, more information in the [next section](#moveit-integration). Additionally it contains a [launch file](../../examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py) that commands 4 goal positions near the home position cyclically (the points and parameters can be modified in [this](../../examples/iiqka_moveit_example/config/dummy_publisher.yaml) configuration file). This can be used to test moving any robot with the driver, and is the recommended way instead of the `rqt_joint_trajectory_controller`, which commands very jerky trajectories due to batching. -The driver is in an experimental state, with only joint position commands supported. We have encountered the following isses: -- If there is an error after starting the launch file, the process must be stopped and started again. This error is related to spawning the joint_trajectory controller and comes forth sporadically. -The logs are the following: +## Moveit integration - [spawner-4] Node not found +The `ros2_control` framework supports Moveit out-of-the-box, as the `joint_trajectory_controller` can interpolate the trajectories planned by it. Setting up Moveit is a little more complex, therefore an example package (`iiqka_moveit_example`) is provided to help developers. - [ERROR] [spawner-4]: process has died [pid ..., exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_trajectory_controller -c /controller_manager -p .../kuka_iiqka_eac_driver/share/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml --inactive --ros-args'] +The package contains a [launch file](../../examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py) that starts the iiQKA driver, `rviz`, and the `move_group` server with the required configuration. The `robot_manager` lifecycle node should be configured and activated after startup. -- If you activate and deactivate the robot manager node, the reactivation will fail, you must restart the whole process if you want to control the robot again +After activation, the Motion Planning plugin can be added (`Add` -> `moveit_ros_visualisation` -> `MotionPlanning`) to plan trajectories from the `rviz` GUI. (`Planning group` in the `Planning` tab should be changed to `manipulator`.) +The package also contains examples of sending planning requests from C++ code, in which case the `rviz` plugin is not necessary. The `MoveitExample` class implements a wrapper around the `MoveGroupInterface`, the example executables in the package use this class to interact with Moveit. Four examples are provided: +- `moveit_basic_planners_example`: the example uses the `PILZ` motion planner to plan a `PTP` and a `LIN` trajectory. It also demonstrates that planning with collision avoidance is not possible with the `PILZ` planner by adding a collision box that invalidates the planned trajectory. +- `moveit_collision_avoidance_example`: the example adds a collision box to the scene and demonstrates, that path planning with collision avoidance is possible using the `OMPL` planning pipeline. +- `moveit_constrained_planning_example`: this example demonstrates constrained planning capabilities, as the planner can find a valid path around the obstacle with the end effector orientation remaining constant (with small tolerance). +- `moveit_depalletizing_example`: this example shows a depalletizing example: a 3x3x3 pallet pattern is added to the scene, the robot can successfully finish the depalletizing process by attaching each pallet to the end effector and moving it to a dropoff position with collision avoidance. +Note: the first three examples should be executed consequently (without restarting the launch file) to ensure that the collision objects are indeed in the way of the trivial path. The 4. example should be executed independently, so that the collision box added in the other examples are not there (launch file should be restarted after the other examples). -## Contact +Note: The examples need user interaction in `rviz`, the `Next` button (`RvizVisualToolsGui` tab) should be pressed each time the logs indicate it. -If you have questions, suggestions or want to contribute, feel free to open an [issue](https://github.com/kroshu/ros2_kuka_sunrise_fri_driver/issues) or start a [discussion](https://github.com/kroshu/ros2_kuka_sunrise_fri_driver/discussions). +## Detailed setup and startup instructions +For more detailed information about the drivers, visit the dedicated wiki pages for [KSS](https://github.com/kroshu/kuka_drivers/wiki/KSS_RSI), [Sunrise](https://github.com/kroshu/kuka_drivers/wiki/Sunrise_FRI) or [iiQKA](https://github.com/kroshu/kuka_drivers/wiki/iiQKA_EAC) robots. diff --git a/doc/wiki/KSS_RSI.md b/doc/wiki/KSS_RSI.md new file mode 100644 index 00000000..7bb29a41 --- /dev/null +++ b/doc/wiki/KSS_RSI.md @@ -0,0 +1,151 @@ +## KSS driver (RSI) + +### Setup + +#### Client side +It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/Realtime)). + +To set up the controller with WorkVisual (which is necessary if RSI is not yet installed), a Windows machine is also required. + +##### Client IP configuration +- Set a fixed IP in the subnet of the KLI interface for the Windows machine, which is required to connect to WorkVisual and transfer the project +- Set a fixed IP in the subnet of the RSI interface for the real-time machine, which is required to send commands via the RSI interface. + +#### Controller side +These instructions were tested with RSI 4.1.3 (on KSS8.6) and RSI 5.0.2 (on KSS8.7) + +##### Controller network configuration + +Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. + +1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). + There should already be an interface checked out as the **Windows interface**. + - **Windows interface checkbox** should be checked. +2. Add a new network for RSI: + + **KRC4:** + - Minimize the SmartHMI (**Start-up > Service > Minimize HMI**). + - Run **RSI-Network** from the Windows Start menu (**All Programs > RSI-Network**). + - Check that the **Network - Kuka User Interface** shows the Windows interface with the specified IP address. + - Add a new IP address on another subnet for the **RSI interface**. + - Select the entry **New** under **RSI Ethernet** in the tree structure and press **Edit**. + - Enter the IP address and confirm with **OK**. + - Close **RSI-Network** and maximize the SmartHMI. + + **KRC5:** + - Press the **Advanced** button and **New interface**. + - Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** + - Set the IP address to a different subnet then the **KLI interface**. + - **Default gateway**: leave it empty + - **Windows interface checkbox** should NOT be checked + +3. Reboot the controller with a cold restart (**Shutdown > Check *Force cold start* and *Reload files* > Reboot control PC**). + +##### Update and upload configuration files +There are 3 files necessary for RSI that are available in the `krl` directory: + +- `ros_rsi_ethernet.xml`: specifies the data transferred via RSI and contains the IP configuration of the client machine: + - The `IP_NUMBER` tag should be modified so that it corresponds to the IP address previously added for your (real-time) PC. + - The `PORT` might be left as it is (59152), but can be also changed if a different port is to be used on the client machine. + +- `ros_rsi.src`: This contains the KRL program that starts external control and should not be modified. +- `ros_rsi.rsix`: This contains the RSI context (can be visualized with **RSIVisual**). It can be modified for example to add filtering behaviour, but this is not recommended and should be implemented on the client side instead. + - For older RSI versions (<=4.0.3), the context can only be defined in 3 different files: `ros_rsi.rsi.xml`, `ros_rsi.rsi.diagram` and `ros_rsi.rsi`, these can be found under `krl/deprecated`. In this case, these 3 files should be copied to the controller instead of the `ros_rsi.rsix`. + +There are two options to upload these files to the controller: + +Method 1: +1. Copy the files to a USB-stick. +2. Plug it into the teach pad or controller. +3. Log in as **Expert** or **Administrator** on the controller. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. +5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. + +Method 2: +1. Connect to the KRC with WorkVisual +2. Log in as **Expert** or **Administrator** on the controller. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in WorkVisual +5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in WorkVisual +6. Deploy the project + +### Configuration + +#### Startup configuration + +The following configuration files are available in the `config` directory of the package: +- `driver_config.yaml`: contains the IP address of the client machine +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- configuration files for specific controllers, for further information, see the documentation of the given controller + +##### IP configuration +The following parameters must be set in the driver configuration file: +- `client_ip`: IP address of the client machine, should be identical to the one set in `ros_rsi_ethernet.xml` +- `client_port`: port of the real-time communication on the client machine, should be identical to the one set in `ros_rsi_ethernet.xml` + +#### Runtime parameters + +The KSS driver currently does not have runtime parameters. Control mode cannot be changed if the driver is running, as that also requires modifying the RSI context on the controller. + +### Usage + +#### Starting the driver + +1. To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run: + ``` + ros2 launch kuka_kss_rsi_driver startup.launch.py + ``` + - This starts the 3 core components of every driver (described in the *Non-real-time interface* section of the [project overview](Project%20overview.md)) and the following controllers: + - `joint_state_broadcaster` (no configuration file, all state interfaces are published) + - `joint_trajectory_controller` ([configuration file](../../kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml)) + + - After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller, see further steps (before this only a collapsed robot is visible in `rviz`): + +2. Configure and activate all components the driver: + ``` + ros2 lifecycle set robot_manager configure + ros2 lifecycle set robot_manager activate + ``` + - The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 10 seconds +3. Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and execute the line of `RSI_MOVECORR()` + - in T1, a warning (*!!! Attention - Sensor correction goes active !!!*) should be visible after reaching `RSI_MOVECORR()`, which should be confirmed to start this step + +On successful activation the brakes of the robot will be released and external control is started. To test moving the robot, the `rqt_joint_trajectory_controller` is not recommended, use the launch file in the `iiqka_moveit_example` package instead (usage is described in the *Additional packages* section of the [project overview](Project%20overview.md)). + + +##### Launch arguments + +Both launch files support the following arguments: +- `robot_model` and `robot_family`: defines which robot to use. The available options for the valid model and family combinations can be found in the [readme](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#what-data-is-verified) of the `kuka_robot_descriptions` repository. +- `use_fake_hardware`: if true, the `mock_components/GenericSystem` will be used instead of the `KukaRSIHardwareInterface`. This enables trying out the driver without actual hardware. + +#### Stopping external control + +To stop external control, all components have to be deactivated with `ros2 lifecycle set robot_manager deactivate` + +BEWARE, that this is a non-realtime process including lifecycle management, so the connection is not terminated immediately, in cases where an abrupt stop is needed, the safety stop of the Teach Pendant should be used! + + +### Simulation + +To try out the driver with an open-loop simulation, the driver and the `kuka_rsi_simulator` must be started, (before activation only a "collapsed" robot will be visible in `rviz`): + +``` +ros2 launch kuka_kss_rsi_driver startup_with_rviz.launch.py +``` + +``` +ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py +``` + +After all components have started successfully, the system needs to be configured and activated to start the simulation. The robot will be visible in rviz after activation: + +``` +ros2 lifecycle set robot_manager configure +ros2 lifecycle set robot_manager activate +``` + +### Known issues and limitations + +- There are currently heap allocations in the control loop (hardware interface `read()` and `write()` functions), therefore the driver is not real-time safe +- In case of an error on the controller side, the driver is not deactivated +- Cartesian position control mode and I/O-s not yet supported diff --git a/doc/wiki/Realtime.md b/doc/wiki/Realtime.md new file mode 100644 index 00000000..0d8d7f53 --- /dev/null +++ b/doc/wiki/Realtime.md @@ -0,0 +1,123 @@ +## Setting up the real-time patch + +### Prerequisites: +- at least 30GB free disk space. + +### Downloading kernel source +1. check your current kernel version + ``` + uname -r + ``` + +2. Find and download the real-time patch with matching major and minor versions from [here](https://cdn.kernel.org/pub/linux/kernel/projects/rt/) (*patch-\<version\>-rt\<nr\>.patch.gz*) + +3. Download the kernel with identical version from [here](https://mirrors.edge.kernel.org/pub/linux/kernel/) (*linux-\<version\>.tar.gz*) + +4. Create directory to build kernel + ``` + mkdir ~/kernel + cd ~/kernel + ``` + - copy the downloaded archives to the `~/kernel` directory + +5. Unpack archives + ``` + tar -xzf linux-<version>.tar.gz + gunzip patch-<version>-rt<nr>.patch.gz + ``` + +6. Patch the kernel with the realtime patch: + ``` + cd linux-<version> + patch -p1 < ../patch-<version>-rt<nr>.patch + ``` + +7. Install dependencies needed for building the kernel + ``` + sudo apt install libncurses-dev flex bison openssl libssl-dev dkms libelf-dev libudev-dev libpci-dev libiberty-dev autoconf fakeroot + ``` + +### Kernel configuration + +#### Create configuration for building the kernel + - The easiest is to use the configuration of the current installation: + ``` + cp /boot/config-<version>-generic .config + ``` + - Modify certificates for signature checking + ``` + scripts/config --disable SYSTEM_REVOCATION_KEYS + scripts/config --disable SYSTEM_REVOCATION_LIST + scripts/config --set-str SYSTEM_TRUSTED_KEYS "" + ``` + - Enable all new Ubuntu configurations: + ``` + yes '' | make oldconfig + ``` + +#### Real-time adaptations + The kernel configuration must be modified to make it real-time-capable. Open GUI for modifications: + ``` + make menuconfig + ``` + +##### Enable fully preemptible kernel + ``` + -> General Setup + -> Preemption Model + (X) Fully Preemptible Kernel (Real-Time) + ``` + +##### Enable high resolution timer support + ``` + -> General setup + -> Timers subsystem + [*] High Resolution Timer Support + ``` + +##### Omit scheduling-clock ticks on isolated CPU-s + ``` + -> General setup + -> Timers subsystem + -> Timer tick handling + (X) Full dynticks system (tickless) + ``` + +##### Enforce maximum CPU frequency + ``` + -> Power management and ACPI options + -> CPU Frequency scaling + -> CPU Frequency scaling + -> Default CPUFreq governor + (X) performance + ``` + +Save (without modifying the name) and exit menuconfig. + +### Build and install kernel + +1. Build the kernel (which will take quite some time): + ``` + make -j `getconf _NPROCESSORS_ONLN` deb-pkg + ``` + After successful completion, there should be 4 debian packages in the `~/kernel` directory + +2. Install all kernel debian packages: + ``` + sudo dpkg -i ../*.deb + ``` + +3. Reboot + - Now the real time kernel should be installed. Reboot the system. + - At startup, choose the built kernel from the boot menu (*Advanced options*), or configure it to be default before restart. + - Check new kernel version with `uname -a` + - you should see `PREEMPT_RT` in the kernel version + +## Configuration + +After installing the real-time kernel, the setting of scheduling priorities must be enabled for your user: +- extend `/etc/security/limits.conf` with +``` +username - rtprio 98 +``` +- Restart the system diff --git a/doc/wiki/Sunrise_FRI.md b/doc/wiki/Sunrise_FRI.md new file mode 100644 index 00000000..1a6e9fad --- /dev/null +++ b/doc/wiki/Sunrise_FRI.md @@ -0,0 +1,77 @@ +## Sunrise driver (FRI) + +### Setup + +#### Client side +- It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/Realtime)). +- Set a fixed IP in the subnet of the controller for the real-time machine. + +#### Controller side + +- Upload the robot application under `robot_application/src` to the controller using the Sunrise Workbench + +### Configuration + +#### Startup configuration + +The following configuration files are available in the `config` directory of the package: +- `driver_config.yaml`: contains IP addresses and runtime parameters +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- configuration files for specific controllers, for further information, see the documentation of the given controller +- `gpio_config.xacro`: contains the I/O setup of the system, but this was not tested yet + +##### IP configuration +The following parameter must be set in the driver configuration file: +- `controller_ip`: IP address of the controller + +#### Runtime parameters +The parameters in the driver configuration file can be also changed during runtime using the parameter interface of the `robot_manager` node: +- `send_period_ms` (integer): this parameter defines the send rate in milliseconds (with which the controller sends robot state updates). It must be between 1 and 10 for control and can be only changed in `inactive` and `configuring` states. +- `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states. +- `control_mode`, `command_mode` (strings): control mode related parameters, which will be combined to support the defined enums. They cannot be changed in active state. +- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. They will be removed after changing to using the `joint_group_impedance_controller` to adapt to conventions. + +### Usage + +#### Starting the driver + +1. On the controller, start the uploaded robot application (ROS2_Control). +2. To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run + ``` + ros2 launch kuka_iiqka_eac_driver startup.launch.py` + ``` + - This starts the 3 core components of every driver (described in the *Non-real-time interface* section of the [project overview](Project%20overview.md)) and the following controllers: + - `joint_state_broadcaster` (no configuration file, all state interfaces are published) + - `joint_trajectory_controller` ([configuration file](../../kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml)) + - [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file) + - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file) + +3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): + ``` + ros2 lifecycle set robot_manager configure + ros2 lifecycle set robot_manager activate + ``` +On successful activation the brakes of the robot will be released and external control is started using the requested control mode. To test moving the robot, the `rqt_joint_trajectory_controller` is not recommended, use the launch file in the `iiqka_moveit_example` package instead (usage is described in the *Additional packages* section of the [project overview](Project%20overview.md)). + + +##### Launch arguments + +Both launch files support the following argument: +- `robot_model`: defines which LBR iiwa robot to use. Available options: `lbr_iiwa14_r820` (default) +- `use_fake_hardware`: if true, the `mock_components/GenericSystem` will be used instead of the `KukaRSIHardwareInterface`. This enables trying out the driver without actual hardware. + +#### Stopping external control + +To stop external control, all components have to be deactivated with `ros2 lifecycle set robot_manager deactivate` + +BEWARE, that this is a non-realtime process including lifecycle management, so the connection is not terminated immediately, in cases where an abrupt stop is needed, the safety stop of the Teach Pendant should be used! (This will also deactivate all components to allow reactivation without a restart.) + + +### Known issues and limitations + +- Not all hardware-related communication is implemented in the hardware interface, therefore the mock hardware option is not working properly +- The control mode handling for the driver is not the one defined in the `kuka_drivers_core` package + - enum definition and controller switching logic is not used + - joint impedance control is not implemented properly using command interfaces +- I/O control was not tested at all +- Cartesian modes are not yet supported diff --git a/doc/wiki/iiQKA_EAC.md b/doc/wiki/iiQKA_EAC.md new file mode 100644 index 00000000..23f14932 --- /dev/null +++ b/doc/wiki/iiQKA_EAC.md @@ -0,0 +1,80 @@ +## iiQKA driver (EAC) + +### Setup + +#### Client side +- It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/Realtime)). +- The driver depends on some KUKA-specific packages, which are only available with the real robot, therefore a mock mode is provided to enable trying out solutions with the same components running. + - By default, the mock libraries are used, this can be changed in the `CmakeLists.txt` file by setting `MOCK_KUKA_LIBS` to `FALSE` before building. +- Set a fixed IP in the subnet of the KONI interface for the real-time machine. + +#### Controller side + +- Install ExternalAPI.Control toolbox (requires iiQKA 1.2) +- Set KONI IP (System Settings -> Network -> KONI) + restart +- Connect external client to KONI port (XF7) +- Release SPOC on the Teach Pendant + +### Configuration + +#### Startup configuration + +The following configuration files are available in the `config` directory of the package: +- `driver_config.yaml`: contains IP addresses and runtime parameters +- `qos_profiles.yaml`: contains the configuration options for the QoS profile defining the connection quality (description later) +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- configuration files for specific controllers, for further information, see the documentation of the given controller + +##### QoS profile configuration +It is possible to configure the required connection quality of external control using a QoS profile, which defines under which conditions should external control be terminated in case of packet losses. A packet is considered lost if the controller does not receive a command in 2.5 milliseconds after publishing the state. In case of a packet loss, extrapolation is used to optimize the robot behaviour, unless the limits defined in the profile are violated. Two limits can be defined: +- Consequent packet losses allowed, defined by `consequent_lost_packets` parameter, maximum value is 5 +- Allowed packet losses in given timeframe: defined by `lost_packets_in_timeframe` (maximum value is 25) and `timeframe_ms` parameters, maximum ratio is 25/sec + +##### IP configuration +The following parameters must be set in the driver configuration file: +- `client_ip`: IP address of the client machine +- `controller_ip`: KONI IP of the controller + +#### Runtime parameters +The not IP-related parameters in the driver configuration file can be also changed during runtime using the parameter interface of the `robot_manager` node: +- `control_mode`: The enum value of the control mode should be given. It can be changed in all primary states, but in active state the brakes are always closed before control is started in a new mode. +- `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state. +- `impedance_controller_name`: The name of the controller (string) that controls the `stiffness` and `damping` interfaces of the robot. It can't be changed in active state. +- `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state. + +### Usage + +#### Starting the driver + +To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run: + +``` +ros2 launch kuka_iiqka_eac_driver startup.launch.py +``` + +This starts the 3 core components of every driver (described in the *Non-real-time interface* section of the [project overview](Project%20overview.md)) and the following controllers: +- `joint_state_broadcaster` (no configuration file, all state interfaces are published) +- `joint_trajectory_controller` ([configuration file](../../kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml)) +- `joint_group_impedance_controller` ([configuration file](../../kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml)) +- `effort_controller` (of type `JointGroupPositionController`, [configuration file](../../kuka_iiqka_eac_driver/config/effort_controller_config.yaml)) +- [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) + +After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): + ``` + ros2 lifecycle set robot_manager configure + ros2 lifecycle set robot_manager activate + ``` + +On successful activation the brakes of the robot will be released and external control is started using the requested control mode. To test moving the robot, the `rqt_joint_trajectory_controller` is not recommended, use the launch file in the `iiqka_moveit_example` package instead (usage is described in the *Additional packages* section of the [project overview](Project%20overview.md)). + + +##### Launch arguments + +Both launch files support the following argument: +- `robot_model`: defines which LBR iisy robot to use. Available options: `lbr_iisy3_r760` (default), `lbr_iisy11_r1300`, `lbr_iisy15_r930` + +#### Stopping external control + +To stop external control, all components have to be deactivated with `ros2 lifecycle set robot_manager deactivate` + +BEWARE, that this is a non-realtime process including lifecycle management, so the connection is not terminated immediately, in cases where an abrupt stop is needed, the safety stop of the Teach Pendant should be used! (This will also deactivate all components to allow reactivation without a restart.) diff --git a/examples/iiqka_moveit_example/CMakeLists.txt b/examples/iiqka_moveit_example/CMakeLists.txt index db9ac4e5..c9e82446 100755 --- a/examples/iiqka_moveit_example/CMakeLists.txt +++ b/examples/iiqka_moveit_example/CMakeLists.txt @@ -74,23 +74,7 @@ install(DIRECTORY config ) if(BUILD_TESTING) - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_pep257 REQUIRED) - find_package(ament_cmake_flake8 REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_xmllint REQUIRED) - ament_copyright() - ament_cppcheck(--language=c++) - ament_pep257() - ament_flake8() - ament_cpplint() - ament_lint_cmake() - ament_uncrustify() - ament_xmllint() endif() ament_package() diff --git a/examples/iiqka_moveit_example/CONTRIBUTING.md b/examples/iiqka_moveit_example/CONTRIBUTING.md index acae8da3..6f63de9e 100644 --- a/examples/iiqka_moveit_example/CONTRIBUTING.md +++ b/examples/iiqka_moveit_example/CONTRIBUTING.md @@ -11,4 +11,3 @@ be under the Apache 2 License, as dictated by that the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. ~~~ - diff --git a/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml b/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml deleted file mode 100644 index f5ebfe95..00000000 --- a/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/publisher_joint_impedance_controller: - ros__parameters: - - controller_name: "joint_group_impedance_controller" - - stiffness_goals: [30, 0, 30, 30, 30, 30] - damping_goals: [0.7, 0.0, 0.7, 0.7, 0.7, 0.7] - - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 diff --git a/examples/iiqka_moveit_example/config/dummy_publisher.yaml b/examples/iiqka_moveit_example/config/dummy_publisher.yaml index 6b2a013f..08f5cb57 100644 --- a/examples/iiqka_moveit_example/config/dummy_publisher.yaml +++ b/examples/iiqka_moveit_example/config/dummy_publisher.yaml @@ -5,13 +5,13 @@ wait_sec_between_publish: 6 goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: + pos1: positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0] - pos2: + pos2: positions: [0.0, -1.37, 0.0, 0.0, 0.0, 0.0] - pos3: + pos3: positions: [0.2, -1.57, 0.2, 0.2, 0.2, 0.2] - pos4: + pos4: positions: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2] joints: diff --git a/examples/iiqka_moveit_example/config/moveit_controllers.yaml b/examples/iiqka_moveit_example/config/moveit_controllers.yaml index 3dd73c3f..486bcb5c 100644 --- a/examples/iiqka_moveit_example/config/moveit_controllers.yaml +++ b/examples/iiqka_moveit_example/config/moveit_controllers.yaml @@ -21,4 +21,3 @@ moveit_simple_controller_manager: - joint_4 - joint_5 - joint_6 - diff --git a/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp b/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp old mode 100755 new mode 100644 index 3634425d..ade40bea --- a/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp +++ b/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp @@ -21,26 +21,22 @@ #include <string> #include <vector> -#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/vector3.hpp" #include "moveit/move_group_interface/move_group_interface.h" #include "moveit/planning_scene_interface/planning_scene_interface.h" #include "moveit_msgs/msg/collision_object.hpp" #include "moveit_visual_tools/moveit_visual_tools.h" -#include "geometry_msgs/msg/vector3.hpp" +#include "rclcpp/rclcpp.hpp" class MoveitExample : public rclcpp::Node { public: - MoveitExample() - : rclcpp::Node("moveit_example") - { - } + MoveitExample() : rclcpp::Node("moveit_example") {} void initialize() { move_group_interface_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>( - shared_from_this(), - PLANNING_GROUP); + shared_from_this(), PLANNING_GROUP); moveit_visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>( shared_from_this(), "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, @@ -50,8 +46,8 @@ class MoveitExample : public rclcpp::Node moveit_visual_tools_->loadRemoteControl(); moveit_visual_tools_->trigger(); - planning_scene_diff_publisher_ = this->create_publisher<moveit_msgs::msg::PlanningScene>( - "planning_scene", 10); + planning_scene_diff_publisher_ = + this->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10); move_group_interface_->setMaxVelocityScalingFactor(0.1); move_group_interface_->setMaxAccelerationScalingFactor(0.1); @@ -70,7 +66,8 @@ class MoveitExample : public rclcpp::Node msg.orientation.w = sqrt(2) / 2; msg.position.x = 0.4; // Define waypoints in a circle - for (int i = 0; i < 63; i++) { + for (int i = 0; i < 63; i++) + { msg.position.y = -0.2 + sin(0.1 * i) * 0.15; msg.position.z = 0.4 + cos(0.1 * i) * 0.15; waypoints.push_back(msg); @@ -81,10 +78,13 @@ class MoveitExample : public rclcpp::Node move_group_interface_->computeCartesianPath(waypoints, 0.005, 0.0, trajectory); RCLCPP_INFO(LOGGER, "Planning done!"); - if (fraction < 1) { + if (fraction < 1) + { RCLCPP_ERROR(LOGGER, "Could not compute trajectory through all waypoints!"); return nullptr; - } else { + } + else + { return std::make_shared<moveit_msgs::msg::RobotTrajectory>(trajectory); } } @@ -101,26 +101,31 @@ class MoveitExample : public rclcpp::Node moveit::planning_interface::MoveGroupInterface::Plan plan; RCLCPP_INFO(LOGGER, "Sending planning request"); - if (!move_group_interface_->plan(plan)) { + if (!move_group_interface_->plan(plan)) + { RCLCPP_INFO(LOGGER, "Planning failed"); return nullptr; - } else { + } + else + { RCLCPP_INFO(LOGGER, "Planning successful"); return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_); } } - moveit_msgs::msg::RobotTrajectory::SharedPtr planToPosition( - const std::vector<double> & joint_pos) + moveit_msgs::msg::RobotTrajectory::SharedPtr planToPosition(const std::vector<double> & joint_pos) { move_group_interface_->setJointValueTarget(joint_pos); moveit::planning_interface::MoveGroupInterface::Plan plan; RCLCPP_INFO(LOGGER, "Sending planning request"); - if (!move_group_interface_->plan(plan)) { + if (!move_group_interface_->plan(plan)) + { RCLCPP_INFO(LOGGER, "Planning failed"); return nullptr; - } else { + } + else + { RCLCPP_INFO(LOGGER, "Planning successful"); return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_); } @@ -140,7 +145,8 @@ class MoveitExample : public rclcpp::Node RCLCPP_INFO(LOGGER, "Sending planning request"); moveit::core::MoveItErrorCode err_code; auto start = std::chrono::high_resolution_clock::now(); - do{ + do + { RCLCPP_INFO(LOGGER, "Planning ..."); err_code = move_group_interface_->plan(plan); } while (err_code != moveit::core::MoveItErrorCode::SUCCESS); @@ -186,8 +192,7 @@ class MoveitExample : public rclcpp::Node } void addCollisionBox( - const geometry_msgs::msg::Vector3 & position, - const geometry_msgs::msg::Vector3 & size) + const geometry_msgs::msg::Vector3 & position, const geometry_msgs::msg::Vector3 & size) { moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = move_group_interface_->getPlanningFrame(); @@ -215,9 +220,12 @@ class MoveitExample : public rclcpp::Node void addPalletObjects() { - for (int k = 0; k < 3; k++) { - for (int j = 0; j < 3; j++) { - for (int i = 0; i < 3; i++) { + for (int k = 0; k < 3; k++) + { + for (int j = 0; j < 3; j++) + { + for (int i = 0; i < 3; i++) + { moveit_msgs::msg::CollisionObject pallet_object; pallet_object.header.frame_id = move_group_interface_->getPlanningFrame(); @@ -299,30 +307,25 @@ class MoveitExample : public rclcpp::Node move_group_interface_->setPathConstraints(constraints); } - void clearConstraints() - { - move_group_interface_->clearPathConstraints(); - } + void clearConstraints() { move_group_interface_->clearPathConstraints(); } void drawTrajectory(const moveit_msgs::msg::RobotTrajectory & trajectory) { moveit_visual_tools_->deleteAllMarkers(); moveit_visual_tools_->publishTrajectoryLine( - trajectory, - moveit_visual_tools_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)); + trajectory, moveit_visual_tools_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)); } void drawTitle(const std::string & text) { auto const text_pose = [] - { - auto msg = Eigen::Isometry3d::Identity(); - msg.translation().z() = 1.0; - return msg; - } (); + { + auto msg = Eigen::Isometry3d::Identity(); + msg.translation().z() = 1.0; + return msg; + }(); moveit_visual_tools_->publishText( - text_pose, text, rviz_visual_tools::RED, - rviz_visual_tools::XXLARGE); + text_pose, text, rviz_visual_tools::RED, rviz_visual_tools::XXLARGE); } void addBreakPoint() diff --git a/examples/iiqka_moveit_example/launch/launch_impedance_publisher.launch.py b/examples/iiqka_moveit_example/launch/launch_impedance_publisher.launch.py deleted file mode 100644 index c57b108c..00000000 --- a/examples/iiqka_moveit_example/launch/launch_impedance_publisher.launch.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2022 Márk Szitanics -# -# 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(): - - impedance_config = PathJoinSubstitution( - [ - FindPackageShare("iiqka_moveit_example"), - "config", - "dummy_impedance_publisher.yaml", - ] - ) - # TODO: create a dummy publisher script in the ros2_control_test_nodes repo - return LaunchDescription( - [ - Node( - package="ros2_controllers_test_nodes", - executable="publisher_joint_impedance_controller", - name="publisher_joint_impedance_controller", - parameters=[impedance_config], - output="both", - ) - ] - ) diff --git a/examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py b/examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py index bfa97259..e7d91d7c 100644 --- a/examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py +++ b/examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py @@ -19,7 +19,6 @@ def generate_launch_description(): - position_goals = PathJoinSubstitution( [ FindPackageShare("iiqka_moveit_example"), diff --git a/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py b/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py index a8d3c660..b4338688 100644 --- a/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py +++ b/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py @@ -18,40 +18,52 @@ from moveit_configs_utils import MoveItConfigsBuilder from launch.actions.include_launch_description import IncludeLaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.launch_description_sources.python_launch_description_source import PythonLaunchDescriptionSource # noqa: E501 +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 from launch.substitutions import LaunchConfiguration def launch_setup(context, *args, **kwargs): - robot_model = LaunchConfiguration('robot_model') + robot_model = LaunchConfiguration("robot_model") moveit_config = ( MoveItConfigsBuilder("kuka_lbr_iisy") - .robot_description(file_path=get_package_share_directory('kuka_lbr_iisy_support') - + "/urdf/{}.urdf.xacro".format(robot_model.perform(context))) - .robot_description_semantic(get_package_share_directory('kuka_lbr_iisy_moveit_config') # noqa: E501 - + "/urdf/{}.srdf".format(robot_model.perform(context))) + .robot_description( + file_path=get_package_share_directory("kuka_lbr_iisy_support") + + f"/urdf/{robot_model.perform(context)}.urdf.xacro" + ) + .robot_description_semantic( + get_package_share_directory("kuka_lbr_iisy_moveit_config") # noqa: E501 + + f"/urdf/{robot_model.perform(context)}.srdf" + ) .robot_description_kinematics(file_path="config/kinematics.yaml") .trajectory_execution(file_path="config/moveit_controllers.yaml") .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) - .joint_limits(file_path=get_package_share_directory('kuka_lbr_iisy_support') - + "/config/{}_joint_limits.yaml".format(robot_model.perform(context))) + .joint_limits( + file_path=get_package_share_directory("kuka_lbr_iisy_support") + + f"/config/{robot_model.perform(context)}_joint_limits.yaml" + ) .to_moveit_configs() ) - rviz_config_file = get_package_share_directory( - 'kuka_resources') + "/config/view_6_axis_urdf.rviz" + rviz_config_file = ( + get_package_share_directory("kuka_resources") + "/config/view_6_axis_planning_scene.rviz" + ) - startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( - [get_package_share_directory('kuka_iiqka_eac_driver'), '/launch/startup.launch.py']), - launch_arguments={'robot_model': "{}".format(robot_model.perform(context))}.items()) + startup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory("kuka_iiqka_eac_driver"), "/launch/startup.launch.py"] + ), + launch_arguments={"robot_model": f"{robot_model.perform(context)}"}.items(), + ) move_group_server = Node( package="moveit_ros_move_group", executable="move_group", output="screen", - parameters=[moveit_config.to_dict(), {'publish_planning_scene_hz': 30.0}], + parameters=[moveit_config.to_dict(), {"publish_planning_scene_hz": 30.0}], ) rviz = Node( @@ -62,19 +74,12 @@ def launch_setup(context, *args, **kwargs): arguments=["-d", rviz_config_file, "--ros-args", "--log-level", "error"], ) - to_start = [ - startup_launch, - move_group_server, - rviz - ] + to_start = [startup_launch, move_group_server, rviz] return to_start def generate_launch_description(): launch_arguments = [] - launch_arguments.append(DeclareLaunchArgument( - 'robot_model', - default_value='lbr_iisy3_r760' - )) + launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760")) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/examples/iiqka_moveit_example/package.xml b/examples/iiqka_moveit_example/package.xml index 401a5d38..4e2fe719 100644 --- a/examples/iiqka_moveit_example/package.xml +++ b/examples/iiqka_moveit_example/package.xml @@ -3,9 +3,9 @@ <package format="3"> <name>iiqka_moveit_example</name> <version>0.0.0</version> - <description>ROS package using the RoX driver</description> + <description>ROS example package using the iiQKA driver and moveit</description> <maintainer email="svastits1@gmail.com">Aron Svastits</maintainer> - <license>BSD</license> + <license>Apache 2.0</license> <buildtool_depend>ament_cmake</buildtool_depend> <build_depend>moveit_common</build_depend> @@ -19,16 +19,7 @@ <exec_depend>ros2_controllers</exec_depend> <exec_depend>moveit</exec_depend> - - <test_depend>ament_lint_auto</test_depend> - <test_depend>ament_lint_common</test_depend> - <test_depend>ament_cmake_copyright</test_depend> - <test_depend>ament_cmake_cppcheck</test_depend> - <test_depend>ament_cmake_pep257</test_depend> - <test_depend>ament_cmake_flake8</test_depend> - <test_depend>ament_cmake_cpplint</test_depend> - <test_depend>ament_cmake_lint_cmake</test_depend> - <test_depend>ament_cmake_xmllint</test_depend> + <exec_depend>ros2_controllers_test_nodes</exec_depend> <export> <build_type>ament_cmake</build_type> diff --git a/examples/iiqka_moveit_example/src/moveit_basic_planners_example.cpp b/examples/iiqka_moveit_example/src/moveit_basic_planners_example.cpp old mode 100755 new mode 100644 index 975a9deb..d840a6e8 --- a/examples/iiqka_moveit_example/src/moveit_basic_planners_example.cpp +++ b/examples/iiqka_moveit_example/src/moveit_basic_planners_example.cpp @@ -18,7 +18,6 @@ #include "iiqka_moveit_example/moveit_example.hpp" - int main(int argc, char * argv[]) { // Setup @@ -27,10 +26,7 @@ int main(int argc, char * argv[]) auto const example_node = std::make_shared<MoveitExample>(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(example_node); - std::thread( - [&executor]() - {executor.spin();}) - .detach(); + std::thread([&executor]() { executor.spin(); }).detach(); example_node->initialize(); example_node->addBreakPoint(); @@ -39,16 +35,13 @@ int main(int argc, char * argv[]) example_node->addRobotPlatform(); // Pilz PTP planner - auto standing_pose = Eigen::Isometry3d( - Eigen::Translation3d( - 0.1, 0, - 0.8) * - Eigen::Quaterniond::Identity()); + auto standing_pose = + Eigen::Isometry3d(Eigen::Translation3d(0.1, 0, 0.8) * Eigen::Quaterniond::Identity()); - auto planned_trajectory = example_node->planToPoint( - standing_pose, - "pilz_industrial_motion_planner", "PTP"); - if (planned_trajectory != nullptr) { + auto planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); @@ -56,14 +49,12 @@ int main(int argc, char * argv[]) example_node->addBreakPoint(); // Pilz LIN planner - auto cart_goal = Eigen::Isometry3d( - Eigen::Translation3d( - 0.4, -0.15, - 0.55) * - Eigen::Quaterniond::Identity()); + auto cart_goal = + Eigen::Isometry3d(Eigen::Translation3d(0.4, -0.15, 0.55) * Eigen::Quaterniond::Identity()); planned_trajectory = example_node->planToPoint(cart_goal, "pilz_industrial_motion_planner", "LIN"); - if (planned_trajectory != nullptr) { + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); @@ -76,23 +67,27 @@ int main(int argc, char * argv[]) example_node->addBreakPoint(); // Try moving back with Pilz LIN - planned_trajectory = example_node->planToPoint( - standing_pose, "pilz_industrial_motion_planner", - "LIN"); - if (planned_trajectory != nullptr) { + planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "LIN"); + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); - } else { + } + else + { example_node->drawTitle("Failed planning with Pilz LIN"); } example_node->addBreakPoint(); // Try moving back with Pilz PTP - planned_trajectory = example_node->planToPoint( - standing_pose, "pilz_industrial_motion_planner", - "PTP"); - if (planned_trajectory != nullptr) { + planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); - } else { + } + else + { example_node->drawTitle("Failed planning with Pilz PTP"); } example_node->addBreakPoint(); diff --git a/examples/iiqka_moveit_example/src/moveit_collision_avoidance_example.cpp b/examples/iiqka_moveit_example/src/moveit_collision_avoidance_example.cpp old mode 100755 new mode 100644 index 006d514b..77021749 --- a/examples/iiqka_moveit_example/src/moveit_collision_avoidance_example.cpp +++ b/examples/iiqka_moveit_example/src/moveit_collision_avoidance_example.cpp @@ -25,10 +25,7 @@ int main(int argc, char * argv[]) auto const example_node = std::make_shared<MoveitExample>(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(example_node); - std::thread( - [&executor]() - {executor.spin();}) - .detach(); + std::thread([&executor]() { executor.spin(); }).detach(); example_node->initialize(); @@ -38,7 +35,8 @@ int main(int argc, char * argv[]) // Go to correct position for the example auto init_trajectory = example_node->planToPosition( std::vector<double>{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587}); - if (init_trajectory != nullptr) { + if (init_trajectory != nullptr) + { example_node->moveGroupInterface()->execute(*init_trajectory); } @@ -48,17 +46,14 @@ int main(int argc, char * argv[]) geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(1.0).z(0.1)); example_node->addBreakPoint(); - auto standing_pose = Eigen::Isometry3d( - Eigen::Translation3d( - 0.1, 0, - 0.8) * - Eigen::Quaterniond::Identity()); + auto standing_pose = + Eigen::Isometry3d(Eigen::Translation3d(0.1, 0, 0.8) * Eigen::Quaterniond::Identity()); // Plan with collision avoidance - auto planned_trajectory = example_node->planToPoint( - standing_pose, "ompl", - "RRTConnectkConfigDefault"); - if (planned_trajectory != nullptr) { + auto planned_trajectory = + example_node->planToPoint(standing_pose, "ompl", "RRTConnectkConfigDefault"); + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); diff --git a/examples/iiqka_moveit_example/src/moveit_constrained_planning_example.cpp b/examples/iiqka_moveit_example/src/moveit_constrained_planning_example.cpp old mode 100755 new mode 100644 index ff79429e..0053d233 --- a/examples/iiqka_moveit_example/src/moveit_constrained_planning_example.cpp +++ b/examples/iiqka_moveit_example/src/moveit_constrained_planning_example.cpp @@ -25,10 +25,7 @@ int main(int argc, char * argv[]) auto const example_node = std::make_shared<MoveitExample>(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(example_node); - std::thread( - [&executor]() - {executor.spin();}) - .detach(); + std::thread([&executor]() { executor.spin(); }).detach(); example_node->initialize(); @@ -38,7 +35,8 @@ int main(int argc, char * argv[]) // Go to correct position for the example auto init_trajectory = example_node->planToPosition( std::vector<double>{0.0017, -2.096, 1.514, 0.0012, -0.9888, -0.0029}); - if (init_trajectory != nullptr) { + if (init_trajectory != nullptr) + { example_node->moveGroupInterface()->execute(*init_trajectory); } @@ -48,11 +46,8 @@ int main(int argc, char * argv[]) geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(1.0).z(0.1)); example_node->addBreakPoint(); - auto cart_goal = Eigen::Isometry3d( - Eigen::Translation3d( - 0.4, -0.15, - 0.55) * - Eigen::Quaterniond::Identity()); + auto cart_goal = + Eigen::Isometry3d(Eigen::Translation3d(0.4, -0.15, 0.55) * Eigen::Quaterniond::Identity()); geometry_msgs::msg::Quaternion q; q.x = 0; @@ -66,7 +61,8 @@ int main(int argc, char * argv[]) // Plan with collision avoidance auto planned_trajectory = example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTkConfigDefault"); - if (planned_trajectory != nullptr) { + if (planned_trajectory != nullptr) + { example_node->drawTrajectory(*planned_trajectory); example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); diff --git a/examples/iiqka_moveit_example/src/moveit_depalletizing_example.cpp b/examples/iiqka_moveit_example/src/moveit_depalletizing_example.cpp old mode 100755 new mode 100644 index b4b1aa72..7b28611f --- a/examples/iiqka_moveit_example/src/moveit_depalletizing_example.cpp +++ b/examples/iiqka_moveit_example/src/moveit_depalletizing_example.cpp @@ -23,23 +23,27 @@ class Depalletizer : public MoveitExample public: void Depalletize() { - for (int k = 0; k < 3; k++) { - for (int j = 0; j < 3; j++) { - for (int i = 0; i < 3; i++) { + for (int k = 0; k < 3; k++) + { + for (int j = 0; j < 3; j++) + { + for (int i = 0; i < 3; i++) + { std::string object_name = "pallet_" + std::to_string(9 * k + 3 * j + i); RCLCPP_INFO(LOGGER, "Going for object %s", object_name.c_str()); // Go to pickup Eigen::Isometry3d pose = Eigen::Isometry3d( - Eigen::Translation3d( - 0.3 + i * 0.1, j * 0.1 - 0.1, - 0.35 - 0.1 * k) * + Eigen::Translation3d(0.3 + i * 0.1, j * 0.1 - 0.1, 0.35 - 0.1 * k) * Eigen::Quaterniond(0, 1, 0, 0)); auto planned_trajectory = planToPointUntilSuccess(pose, "ompl", "RRTConnectkConfigDefault"); - if (planned_trajectory != nullptr) { + if (planned_trajectory != nullptr) + { move_group_interface_->execute(*planned_trajectory); - } else { + } + else + { RCLCPP_ERROR(LOGGER, "Planning failed"); } @@ -49,12 +53,14 @@ class Depalletizer : public MoveitExample // Drop off to -0.3, 0.0, 0.35 pointing down Eigen::Isometry3d dropoff_pose = Eigen::Isometry3d( Eigen::Translation3d(-0.3, 0.0, 0.35) * Eigen::Quaterniond(0, 1, 0, 0)); - auto drop_trajectory = planToPointUntilSuccess( - dropoff_pose, "ompl", - "RRTConnectkConfigDefault"); - if (drop_trajectory != nullptr) { + auto drop_trajectory = + planToPointUntilSuccess(dropoff_pose, "ompl", "RRTConnectkConfigDefault"); + if (drop_trajectory != nullptr) + { move_group_interface_->execute(*drop_trajectory); - } else { + } + else + { RCLCPP_ERROR(LOGGER, "Planning failed"); } @@ -74,10 +80,7 @@ int main(int argc, char * argv[]) auto const node = std::make_shared<Depalletizer>(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); - std::thread( - [&executor]() - {executor.spin();}) - .detach(); + std::thread([&executor]() { executor.spin(); }).detach(); node->initialize(); diff --git a/kuka_driver_interfaces/CMakeLists.txt b/kuka_driver_interfaces/CMakeLists.txt index 9976f108..b921b7dc 100644 --- a/kuka_driver_interfaces/CMakeLists.txt +++ b/kuka_driver_interfaces/CMakeLists.txt @@ -31,8 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + endif() ament_package() diff --git a/kuka_driver_interfaces/msg/FRIState.msg b/kuka_driver_interfaces/msg/FRIState.msg index 8b18d3f9..5586374e 100644 --- a/kuka_driver_interfaces/msg/FRIState.msg +++ b/kuka_driver_interfaces/msg/FRIState.msg @@ -1,6 +1,6 @@ # Message describing the state of the robot and FRI -# FRI session state enum +# FRI session state enum # IDLE = 0, MONITORING_WAIT = 1, MONITORING_READY = 2,COMMANDING_WAIT = 3, COMMANDING_ACTIVE = 4 int32 session_state diff --git a/kuka_driver_interfaces/package.xml b/kuka_driver_interfaces/package.xml index 42903baf..57e737f9 100644 --- a/kuka_driver_interfaces/package.xml +++ b/kuka_driver_interfaces/package.xml @@ -10,11 +10,8 @@ <buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>rosidl_default_generators</buildtool_depend> - <test_depend>ament_lint_auto</test_depend> - <test_depend>ament_lint_common</test_depend> - <depend>geometry_msgs</depend> - + <exec_depend>rosidl_default_runtime</exec_depend> <member_of_group>rosidl_interface_packages</member_of_group> diff --git a/kuka_drivers/CMakeLists.txt b/kuka_drivers/CMakeLists.txt index 4b8f9700..395bca9f 100644 --- a/kuka_drivers/CMakeLists.txt +++ b/kuka_drivers/CMakeLists.txt @@ -7,4 +7,4 @@ endif() find_package(ament_cmake REQUIRED) -ament_package() \ No newline at end of file +ament_package() diff --git a/kuka_drivers/package.xml b/kuka_drivers/package.xml index ec9af207..8436e6a0 100644 --- a/kuka_drivers/package.xml +++ b/kuka_drivers/package.xml @@ -2,10 +2,10 @@ <package format="2"> <name>kuka_drivers</name> <version>0.0.1</version> - <description>ROS2 Controllers for KUKA robots</description> + <description>ROS2 drivers for KUKA robots</description> <author>Áron Svastits</author> <maintainer email="svastits1@gmail.com">Áron Svastits</maintainer> - <license>BSD</license> + <license>Apache 2.0</license> <url type="bugtracker">https://github.com/kroshu/kuka_drivers/issues</url> <url type="repository">https://github.com/kroshu/kuka_drivers</url> diff --git a/kuka_drivers_core/CMakeLists.txt b/kuka_drivers_core/CMakeLists.txt index 2a73eb83..48a21295 100644 --- a/kuka_drivers_core/CMakeLists.txt +++ b/kuka_drivers_core/CMakeLists.txt @@ -71,23 +71,7 @@ install(TARGETS ${PROJECT_NAME} control_node ament_export_include_directories(include) if(BUILD_TESTING) - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_pep257 REQUIRED) - find_package(ament_cmake_flake8 REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_xmllint REQUIRED) - - ament_copyright() - ament_cppcheck(--language=c++) - ament_pep257() - ament_flake8() - ament_cpplint() - ament_lint_cmake() - ament_uncrustify() - ament_xmllint() + endif() ament_package() diff --git a/kuka_drivers_core/README.md b/kuka_drivers_core/README.md index 9cf8140d..efbdeb41 100644 --- a/kuka_drivers_core/README.md +++ b/kuka_drivers_core/README.md @@ -1,5 +1,5 @@ # Core classes which help function the repositories of kroshu. -These classes provide functonalities which are frequently used in ROS2 environment. +These classes provide functionalities which are frequently used in ROS2 environment. Deriving from these classes the user has a helpful wrapper around the base functionalities of ROS2. Right now there are two classes implemented in this repository, ROS2BaseNode for simple parameter handling, and ROS2BaseLCNode, which additionally furthers the rclcpp_lifecycle::LifecycleNode class by implementing lifecycle functions which would be usually implemented in the same way in every case. These are virtual functions, so it is possible to override them in the case of a different desired implementation. diff --git a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp index 259d17cc..4054ada3 100644 --- a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp +++ b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp @@ -15,14 +15,14 @@ #ifndef COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_ #define COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_ +#include <memory> #include <string> #include <vector> -#include <memory> -#include "rclcpp/rclcpp.hpp" +#include "communication_helpers/service_tools.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" -#include "communication_helpers/service_tools.hpp" +#include "rclcpp/rclcpp.hpp" namespace kuka_drivers_core { @@ -37,7 +37,8 @@ bool changeHardwareState( hw_request->target_state.id = state; auto hw_response = sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>( client, hw_request, 0, timeout_ms); - if (!hw_response || !hw_response->ok) { + if (!hw_response || !hw_response->ok) + { return false; } return true; @@ -57,7 +58,8 @@ bool changeControllerState( auto controller_response = sendRequest<controller_manager_msgs::srv::SwitchController::Response>( client, controller_request, 0, 2000); - if (!controller_response || !controller_response->ok) { + if (!controller_response || !controller_response->ok) + { return false; } return true; diff --git a/kuka_drivers_core/include/communication_helpers/serialization.hpp b/kuka_drivers_core/include/communication_helpers/serialization.hpp index 7aa3b96a..3433ad2d 100644 --- a/kuka_drivers_core/include/communication_helpers/serialization.hpp +++ b/kuka_drivers_core/include/communication_helpers/serialization.hpp @@ -15,9 +15,9 @@ #ifndef COMMUNICATION_HELPERS__SERIALIZATION_HPP_ #define COMMUNICATION_HELPERS__SERIALIZATION_HPP_ -#include <vector> -#include <cstdint> #include <algorithm> +#include <cstdint> +#include <vector> namespace kuka_drivers_core { @@ -32,13 +32,14 @@ int serializeNext(int integer_in, std::vector<std::uint8_t> & serialized_out) auto from_it = std::prev(serialized_out.end(), sizeof(int)); std::reverse(from_it, serialized_out.end()); // TODO(resizoltan): assert that int is 4 bytes long - // TODO(resizoltan): check endiannes + // TODO(resizoltan): check endianness return sizeof(int); } int deserializeNext(const std::vector<std::uint8_t> & serialized_in, int & integer_out) { - if (serialized_in.size() < sizeof(int)) { + if (serialized_in.size() < sizeof(int)) + { // TODO(resizoltan): error } std::vector<std::uint8_t> serialized_copy = serialized_in; @@ -55,13 +56,14 @@ int serializeNext(double double_in, std::vector<std::uint8_t> & serialized_out) auto from_it = std::prev(serialized_out.end(), sizeof(double)); std::reverse(from_it, serialized_out.end()); // TODO(resizoltan): assert that int is 4 bytes long - // TODO(resizoltan): check endiannes + // TODO(resizoltan): check endianness return sizeof(int); } int deserializeNext(const std::vector<std::uint8_t> & serialized_in, double & double_out) { - if (serialized_in.size() < sizeof(double)) { + if (serialized_in.size() < sizeof(double)) + { // TODO(resizoltan): error } std::vector<std::uint8_t> serialized_copy = serialized_in; diff --git a/kuka_drivers_core/include/communication_helpers/service_tools.hpp b/kuka_drivers_core/include/communication_helpers/service_tools.hpp index 03ac7097..c2714ca7 100644 --- a/kuka_drivers_core/include/communication_helpers/service_tools.hpp +++ b/kuka_drivers_core/include/communication_helpers/service_tools.hpp @@ -23,16 +23,18 @@ namespace kuka_drivers_core { -template<typename FutureT, typename WaitTimeT> +template <typename FutureT, typename WaitTimeT> std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) { auto end = std::chrono::steady_clock::now() + time_to_wait; std::chrono::milliseconds wait_period(100); std::future_status status = std::future_status::timeout; - do { + do + { auto now = std::chrono::steady_clock::now(); auto time_left = end - now; - if (time_left <= std::chrono::seconds(0)) { + if (time_left <= std::chrono::seconds(0)) + { break; } status = future.wait_for((time_left < wait_period) ? time_left : wait_period); @@ -40,22 +42,22 @@ std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) return status; } -template<typename ResponseT, typename RequestT, typename ClientT> -std::shared_ptr<ResponseT> -sendRequest( +template <typename ResponseT, typename RequestT, typename ClientT> +std::shared_ptr<ResponseT> sendRequest( ClientT client, RequestT request, const uint32_t & service_timeout_ms = 2000, const uint32_t & response_timeout_ms = 100) { - if (service_timeout_ms && !client->wait_for_service( - std::chrono::milliseconds(service_timeout_ms))) + if ( + service_timeout_ms && !client->wait_for_service(std::chrono::milliseconds(service_timeout_ms))) { printf("Wait for service failed\n"); return nullptr; } auto future_result = client->async_send_request(request); - auto future_status = wait_for_result( - future_result, std::chrono::milliseconds(response_timeout_ms)); - if (future_status != std::future_status::ready) { + auto future_status = + wait_for_result(future_result, std::chrono::milliseconds(response_timeout_ms)); + if (future_status != std::future_status::ready) + { printf("Request timed out\n"); return nullptr; } diff --git a/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp index 4a743ba4..01111653 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_ #define KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_ namespace kuka_drivers_core { /** - * @brief Enum to identify every control mode - */ + * @brief Enum to identify every control mode + */ enum class ControlMode : std::uint8_t { UNSPECIFIED_CONTROL_MODE = 0, @@ -34,10 +33,9 @@ enum class ControlMode : std::uint8_t WRENCH_CONTROL = 8, }; - /** - * @brief Enum for identify every type of controllers - */ + * @brief Enum for identify every type of controllers + */ enum class ControllerType : std::uint8_t { JOINT_POSITION_CONTROLLER_TYPE = 0, diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp index 4eeb7850..29feecd9 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp @@ -15,14 +15,14 @@ #ifndef KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ #define KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ -#include <string> -#include <vector> #include <map> -#include <utility> #include <set> +#include <string> +#include <utility> +#include <vector> -#include "rclcpp/rclcpp.hpp" #include "control_mode.hpp" +#include "rclcpp/rclcpp.hpp" namespace kuka_drivers_core { @@ -45,7 +45,7 @@ class ControllerHandler }; /** - * @brief Controller names thats have to be active in all control modes + * @brief Controller names that's have to be active in all control modes */ std::set<std::string> fixed_controllers_; @@ -86,19 +86,21 @@ class ControllerHandler /** * @brief Updates the controllers' name for a specific controller type. * - * @param controller_type: The type of the controller wich will be updated. - * @param controller_name: The new controller's name. From now on this controller will be activated on controller acivation. + * @param controller_type: The type of the controller which will be updated. + * @param controller_name: The new controller's name. From now on this controller will be + * activated on controller activation. * @return True, if update was successful. * @return False, if update failed. */ bool UpdateControllerName( - const ControllerType controller_type, - const std::string & controller_name); + const ControllerType controller_type, const std::string & controller_name); /** - * @brief Calculates the controllers that have to be activated and deactivated for the control mode change + * @brief Calculates the controllers that have to be activated and deactivated for the control + * mode change * - * @param new_control_mode: The new control mode. It is based on Controller_handler::control_mode enum. + * @param new_control_mode: The new control mode. It is based on Controller_handler::control_mode + * enum. * @return std::pair<std::vector<std::string>, std::vector<std::string>>: * Two vectors, first has the controllers to activate, second has the controllers to deactivate * @exception std::out_of_range: new_control_mode attribute is invalid @@ -127,5 +129,4 @@ class ControllerHandler }; } // namespace kuka_drivers_core - #endif // KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index 34177c6d..b1d236c4 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef KUKA_DRIVERS_CORE__HARDWARE_INTERFACE_TYPES_HPP_ #define KUKA_DRIVERS_CORE__HARDWARE_INTERFACE_TYPES_HPP_ @@ -50,7 +49,6 @@ static constexpr char DRIVE_STATE[] = "drive_state"; static constexpr char OVERLAY_TYPE[] = "overlay_type"; static constexpr char TRACKING_PERFORMANCE[] = "tracking_performance"; - } // namespace hardware_interface #endif // KUKA_DRIVERS_CORE__HARDWARE_INTERFACE_TYPES_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp index ec1f5b13..f5db7a02 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp @@ -20,10 +20,9 @@ #include <string> #include <vector> +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" - namespace kuka_drivers_core { @@ -36,7 +35,8 @@ struct ParameterSetAccessRights bool configuring = false; bool isSetAllowed(std::uint8_t current_state) const { - switch (current_state) { + switch (current_state) + { case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: return unconfigured; case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: @@ -57,7 +57,7 @@ class ParameterHandler { class ParameterBase { -public: + public: ParameterBase( const std::string & name, const ParameterSetAccessRights & rights, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr param_IF) @@ -67,20 +67,11 @@ class ParameterHandler virtual ~ParameterBase() = default; - const std::string & getName() const - { - return name_; - } + const std::string & getName() const { return name_; } - const ParameterSetAccessRights & getRights() const - { - return rights_; - } + const ParameterSetAccessRights & getRights() const { return rights_; } - const rclcpp::ParameterValue & getDefaultValue() const - { - return default_value_; - } + const rclcpp::ParameterValue & getDefaultValue() const { return default_value_; } rclcpp::node_interfaces::NodeParametersInterface::SharedPtr getParameterInterface() const { @@ -89,25 +80,22 @@ class ParameterHandler virtual void blockParameter() = 0; - virtual bool callCallback(const rclcpp::Parameter &) const {return false;} + virtual bool callCallback(const rclcpp::Parameter &) const { return false; } -protected: - void setDefaultValue(rclcpp::ParameterValue && value) - { - default_value_ = value; - } + protected: + void setDefaultValue(rclcpp::ParameterValue && value) { default_value_ = value; } const std::string name_; -private: + private: const ParameterSetAccessRights rights_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr paramIF_; rclcpp::ParameterValue default_value_; }; - template<typename T> + template <typename T> class Parameter : public ParameterBase { -public: + public: Parameter( const std::string & name, const T & value, const ParameterSetAccessRights & rights, std::function<bool(const T &)> on_change_callback, @@ -121,9 +109,12 @@ class ParameterHandler bool callCallback(const rclcpp::Parameter & new_param) const override { - try { + try + { return on_change_callback_(new_param.get_value<T>()); - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { printf("%s", e.what()); return false; } @@ -132,13 +123,13 @@ class ParameterHandler void blockParameter() override { on_change_callback_ = [this](const T &) -> bool - { - printf("Parameter %s can be set only at startup\n", name_.c_str()); - return false; - }; + { + printf("Parameter %s can be set only at startup\n", name_.c_str()); + return false; + }; } -private: + private: std::function<bool(const T &)> on_change_callback_; }; @@ -149,26 +140,23 @@ class ParameterHandler const std::vector<rclcpp::Parameter> & parameters) const; bool canSetParameter(const ParameterBase & param) const; - template<typename T> + template <typename T> void registerParameter( const std::string & name, const T & value, const ParameterSetAccessRights & rights, std::function<bool(const T &)> on_change_callback, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr param_IF, bool block = false) { auto param_shared_ptr = std::make_shared<ParameterHandler::Parameter<T>>( - name, value, rights, - on_change_callback, param_IF); + name, value, rights, on_change_callback, param_IF); registerParameter(param_shared_ptr, block); } - template<typename T> + template <typename T> void registerParameter( - const std::string & name, const T & value, - std::function<bool(const T &)> on_change_callback, + const std::string & name, const T & value, std::function<bool(const T &)> on_change_callback, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr param_IF, bool block = false) { auto param_shared_ptr = std::make_shared<ParameterHandler::Parameter<T>>( - name, value, ParameterSetAccessRights(), - on_change_callback, param_IF); + name, value, ParameterSetAccessRights(), on_change_callback, param_IF); registerParameter(param_shared_ptr, block); } @@ -179,5 +167,4 @@ class ParameterHandler }; } // namespace kuka_drivers_core - #endif // KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp index f2e573b2..7ed83c4f 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_lc_node.hpp @@ -15,13 +15,13 @@ #ifndef KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ #define KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ -#include <string> #include <map> -#include <vector> #include <memory> +#include <string> +#include <vector> -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "kuka_drivers_core/parameter_handler.hpp" @@ -31,45 +31,42 @@ class ROS2BaseLCNode : public rclcpp_lifecycle::LifecycleNode { public: explicit ROS2BaseLCNode( - const std::string & node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_error(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State &) override; - template<typename T> + template <typename T> void registerParameter( const std::string & name, const T & value, const ParameterSetAccessRights & rights, std::function<bool(const T &)> on_change_callback) { param_handler_.registerParameter<T>( - name, value, rights, - on_change_callback, this->get_node_parameters_interface()); + name, value, rights, on_change_callback, this->get_node_parameters_interface()); } - template<typename T> + template <typename T> void registerStaticParameter( const std::string & name, const T & value, const ParameterSetAccessRights & rights, std::function<bool(const T &)> on_change_callback) { param_handler_.registerParameter<T>( - name, value, rights, - on_change_callback, this->get_node_parameters_interface(), true); + name, value, rights, on_change_callback, this->get_node_parameters_interface(), true); } const ParameterHandler & getParameterHandler() const; @@ -89,5 +86,4 @@ class ROS2BaseLCNode : public rclcpp_lifecycle::LifecycleNode } // namespace kuka_drivers_core - #endif // KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp index ebce99e0..7cda81dd 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/ros2_base_node.hpp @@ -15,14 +15,14 @@ #ifndef KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_ #define KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_ -#include <string> +#include <functional> #include <map> -#include <vector> #include <memory> -#include <functional> +#include <string> +#include <vector> -#include "rclcpp/node.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "kuka_drivers_core/parameter_handler.hpp" @@ -32,28 +32,23 @@ class ROS2BaseNode : public rclcpp::Node { public: explicit ROS2BaseNode( - const std::string & node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); const ParameterHandler & getParameterHandler() const; - template<typename T> + template <typename T> void registerParameter( - const std::string & name, const T & value, - std::function<bool(const T &)> on_change_callback) + const std::string & name, const T & value, std::function<bool(const T &)> on_change_callback) { param_handler_.registerParameter<T>( - name, value, - on_change_callback, this->get_node_parameters_interface()); + name, value, on_change_callback, this->get_node_parameters_interface()); } - template<typename T> + template <typename T> void registerStaticParameter( - const std::string & name, const T & value, - std::function<bool(const T &)> on_change_callback) + const std::string & name, const T & value, std::function<bool(const T &)> on_change_callback) { param_handler_.registerParameter<T>( - name, value, - on_change_callback, this->get_node_parameters_interface(), true); + name, value, on_change_callback, this->get_node_parameters_interface(), true); } protected: diff --git a/kuka_drivers_core/package.xml b/kuka_drivers_core/package.xml index 1db66620..4b0d9d56 100644 --- a/kuka_drivers_core/package.xml +++ b/kuka_drivers_core/package.xml @@ -15,15 +15,6 @@ <depend>lifecycle_msgs</depend> <depend>controller_manager</depend> - <test_depend>ament_cmake_copyright</test_depend> - <test_depend>ament_cmake_cppcheck</test_depend> - <test_depend>ament_cmake_pep257</test_depend> - <test_depend>ament_cmake_flake8</test_depend> - <test_depend>ament_cmake_cpplint</test_depend> - <test_depend>ament_cmake_lint_cmake</test_depend> - <test_depend>ament_cmake_xmllint</test_depend> - <test_depend>ament_cmake_uncrustify</test_depend> - <export> <build_type>ament_cmake</build_type> </export> diff --git a/kuka_drivers_core/src/control_node.cpp b/kuka_drivers_core/src/control_node.cpp index e205e9ad..7ebf36f9 100644 --- a/kuka_drivers_core/src/control_node.cpp +++ b/kuka_drivers_core/src/control_node.cpp @@ -12,21 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include <thread> #include <memory> +#include <thread> #include "controller_manager/controller_manager.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" - int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(); - auto controller_manager = std::make_shared<controller_manager::ControllerManager>( - executor, - "controller_manager"); + auto controller_manager = + std::make_shared<controller_manager::ControllerManager>(executor, "controller_manager"); auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); qos.best_effort(); @@ -34,15 +32,15 @@ int main(int argc, char ** argv) std::atomic_bool is_configured = false; auto is_configured_sub = controller_manager->create_subscription<std_msgs::msg::Bool>( "robot_manager/is_configured", qos, - [&is_configured](std_msgs::msg::Bool::SharedPtr msg) { - is_configured = msg->data; - }); + [&is_configured](std_msgs::msg::Bool::SharedPtr msg) { is_configured = msg->data; }); - - std::thread control_loop([controller_manager, &is_configured]() { + std::thread control_loop( + [controller_manager, &is_configured]() + { struct sched_param param; param.sched_priority = 95; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { + if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) + { RCLCPP_ERROR(controller_manager->get_logger(), "setscheduler error"); RCLCPP_ERROR(controller_manager->get_logger(), strerror(errno)); RCLCPP_WARN( @@ -51,24 +49,30 @@ int main(int argc, char ** argv) } const rclcpp::Duration dt = - rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); - std::chrono::milliseconds dt_ms {1000 / controller_manager->get_update_rate()}; + rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); + std::chrono::milliseconds dt_ms{1000 / controller_manager->get_update_rate()}; - try { - while (rclcpp::ok()) { - if (is_configured) { + try + { + while (rclcpp::ok()) + { + if (is_configured) + { controller_manager->read(controller_manager->now(), dt); controller_manager->update(controller_manager->now(), dt); controller_manager->write(controller_manager->now(), dt); - } else { + } + else + { controller_manager->update(controller_manager->now(), dt); std::this_thread::sleep_for(dt_ms); } } - } catch (std::exception & e) { + } + catch (std::exception & e) + { RCLCPP_ERROR( - controller_manager->get_logger(), "Quitting control loop due to: %s", - e.what()); + controller_manager->get_logger(), "Quitting control loop due to: %s", e.what()); } }); diff --git a/kuka_drivers_core/src/controller_handler.cpp b/kuka_drivers_core/src/controller_handler.cpp index 0438c837..1cf13999 100644 --- a/kuka_drivers_core/src/controller_handler.cpp +++ b/kuka_drivers_core/src/controller_handler.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. #include <string> -#include <vector> #include <utility> +#include <vector> #include "kuka_drivers_core/controller_handler.hpp" @@ -21,18 +21,17 @@ namespace kuka_drivers_core { ControllerHandler::ControllerHandler(std::vector<std::string> fixed_controllers) : fixed_controllers_(fixed_controllers.begin(), fixed_controllers.end()) -{} +{ +} bool ControllerHandler::UpdateControllerName( - const ControllerType controller_type, - const std::string & controller_name) + const ControllerType controller_type, const std::string & controller_name) { - switch (controller_type) { + switch (controller_type) + { case ControllerType::JOINT_POSITION_CONTROLLER_TYPE: - control_mode_map_[ControlMode::JOINT_POSITION_CONTROL].standard_controller = - controller_name; - control_mode_map_[ControlMode::JOINT_IMPEDANCE_CONTROL].standard_controller = - controller_name; + control_mode_map_[ControlMode::JOINT_POSITION_CONTROL].standard_controller = controller_name; + control_mode_map_[ControlMode::JOINT_IMPEDANCE_CONTROL].standard_controller = controller_name; break; case ControllerType::CARTESIAN_POSITION_CONTROLLER_TYPE: control_mode_map_[ControlMode::CARTESIAN_POSITION_CONTROL].standard_controller = @@ -41,8 +40,7 @@ bool ControllerHandler::UpdateControllerName( controller_name; break; case ControllerType::JOINT_VELOCITY_CONTROLLER_TYPE: - control_mode_map_[ControlMode::JOINT_VELOCITY_CONTROL].standard_controller = - controller_name; + control_mode_map_[ControlMode::JOINT_VELOCITY_CONTROL].standard_controller = controller_name; break; case ControllerType::TWIST_CONTROLLER_TYPE: control_mode_map_[ControlMode::CARTESIAN_VELOCITY_CONTROL].standard_controller = @@ -57,12 +55,10 @@ bool ControllerHandler::UpdateControllerName( controller_name; break; case ControllerType::TORQUE_CONTROLLER_TYPE: - control_mode_map_[ControlMode::JOINT_TORQUE_CONTROL].standard_controller = - controller_name; + control_mode_map_[ControlMode::JOINT_TORQUE_CONTROL].standard_controller = controller_name; break; case ControllerType::WRENCH_CONTROLLER_TYPE: - control_mode_map_[ControlMode::WRENCH_CONTROL].standard_controller = - controller_name; + control_mode_map_[ControlMode::WRENCH_CONTROL].standard_controller = controller_name; break; default: RCLCPP_INFO(rclcpp::get_logger("ControllerHandler"), "Invalid Controller type"); @@ -74,20 +70,23 @@ bool ControllerHandler::UpdateControllerName( std::pair<std::vector<std::string>, std::vector<std::string>> ControllerHandler::GetControllersForSwitch(ControlMode new_control_mode) { - if (control_mode_map_.find(new_control_mode) == control_mode_map_.end()) { + if (control_mode_map_.find(new_control_mode) == control_mode_map_.end()) + { // Not valid control mode, through error throw std::out_of_range("Attribute new_control_mode is out of range"); } - if (new_control_mode == ControlMode::UNSPECIFIED_CONTROL_MODE) { + if (new_control_mode == ControlMode::UNSPECIFIED_CONTROL_MODE) + { throw std::logic_error("UNSPECIFIED_CONTROL_MODE is not valid control mode"); } - // Set controllers wich should be activated and deactivated + // Set controllers which should be activated and deactivated activate_controllers_.clear(); auto control_mode_controllers = control_mode_map_.at(new_control_mode); activate_controllers_.insert(control_mode_controllers.standard_controller); - if (!control_mode_controllers.impedance_controller.empty()) { + if (!control_mode_controllers.impedance_controller.empty()) + { activate_controllers_.insert(control_mode_controllers.impedance_controller); } @@ -95,30 +94,27 @@ ControllerHandler::GetControllersForSwitch(ControlMode new_control_mode) deactivate_controllers_ = active_controllers_; - // Goes through every controllers that should be activated for (auto activate_controllers_it = activate_controllers_.begin(); - activate_controllers_it != activate_controllers_.end(); ) + activate_controllers_it != activate_controllers_.end();) { // Finds the controller in the deactivate controllers - auto deactivate_controllers_it = - deactivate_controllers_.find(*activate_controllers_it); - if (deactivate_controllers_it != deactivate_controllers_.end()) { - // Delete those controllers wich not need to be activated or deactivated. + auto deactivate_controllers_it = deactivate_controllers_.find(*activate_controllers_it); + if (deactivate_controllers_it != deactivate_controllers_.end()) + { + // Delete those controllers which not need to be activated or deactivated. activate_controllers_it = activate_controllers_.erase(activate_controllers_it); deactivate_controllers_.erase(deactivate_controllers_it); - } else { + } + else + { ++activate_controllers_it; } } return std::make_pair( - std::vector<std::string>( - activate_controllers_.begin(), - activate_controllers_.end()), - std::vector<std::string>( - deactivate_controllers_.begin(), - deactivate_controllers_.end())); + std::vector<std::string>(activate_controllers_.begin(), activate_controllers_.end()), + std::vector<std::string>(deactivate_controllers_.begin(), deactivate_controllers_.end())); } std::vector<std::string> ControllerHandler::GetControllersForDeactivation() @@ -129,7 +125,8 @@ std::vector<std::string> ControllerHandler::GetControllersForDeactivation() void ControllerHandler::ApproveControllerActivation() { - if (!activate_controllers_.empty()) { + if (!activate_controllers_.empty()) + { active_controllers_.insert(activate_controllers_.begin(), activate_controllers_.end()); activate_controllers_.clear(); } @@ -137,9 +134,11 @@ void ControllerHandler::ApproveControllerActivation() bool ControllerHandler::ApproveControllerDeactivation() { - for (auto && controller : deactivate_controllers_) { + for (auto && controller : deactivate_controllers_) + { auto active_controller_it = active_controllers_.find(controller); - if (active_controller_it == active_controllers_.end()) { + if (active_controller_it == active_controllers_.end()) + { // We should not reach this, active controllers should always contain the ones to deactivate return false; } @@ -149,4 +148,4 @@ bool ControllerHandler::ApproveControllerDeactivation() return true; } -} // namespace kuka_drivers_core +} // namespace kuka_drivers_core diff --git a/kuka_drivers_core/src/parameter_handler.cpp b/kuka_drivers_core/src/parameter_handler.cpp index 83cfb58b..1cff09e5 100644 --- a/kuka_drivers_core/src/parameter_handler.cpp +++ b/kuka_drivers_core/src/parameter_handler.cpp @@ -14,33 +14,32 @@ #include "kuka_drivers_core/parameter_handler.hpp" +#include <memory> #include <string> #include <vector> -#include <memory> namespace kuka_drivers_core { -ParameterHandler::ParameterHandler(rclcpp_lifecycle::LifecycleNode * node) -: node_(node) -{ -} +ParameterHandler::ParameterHandler(rclcpp_lifecycle::LifecycleNode * node) : node_(node) {} rcl_interfaces::msg::SetParametersResult ParameterHandler::onParamChange( const std::vector<rclcpp::Parameter> & parameters) const { rcl_interfaces::msg::SetParametersResult result; result.successful = false; - for (const rclcpp::Parameter & param : parameters) { + for (const rclcpp::Parameter & param : parameters) + { auto found_param_it = std::find_if( params_.begin(), params_.end(), - [¶m](auto param_ptr) { - return param_ptr->getName() == param.get_name(); - }); + [¶m](auto param_ptr) { return param_ptr->getName() == param.get_name(); }); // When used properly, we should not reach this // but better to keep additional check to filter improper use - if (found_param_it == params_.end()) { + if (found_param_it == params_.end()) + { printf("Invalid parameter name\n"); - } else if (canSetParameter(*(*found_param_it))) { + } + else if (canSetParameter(*(*found_param_it))) + { result.successful = (*found_param_it)->callCallback(param); } } @@ -49,22 +48,25 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::onParamChange( bool ParameterHandler::canSetParameter(const ParameterBase & param) const { - if (node_ == nullptr) { - // Node is not lifecycle node, paramater can always be set + if (node_ == nullptr) + { + // Node is not lifecycle node, parameter can always be set return true; } - try { - if (!param.getRights().isSetAllowed(node_->get_current_state().id())) { + try + { + if (!param.getRights().isSetAllowed(node_->get_current_state().id())) + { RCLCPP_ERROR( - node_->get_logger(), - "Parameter %s cannot be changed while in state %s", + node_->get_logger(), "Parameter %s cannot be changed while in state %s", param.getName().c_str(), node_->get_current_state().label().c_str()); return false; } - } catch (const std::out_of_range &) { + } + catch (const std::out_of_range &) + { RCLCPP_ERROR( - node_->get_logger(), - "Parameter set access rights for parameter %s couldn't be determined", + node_->get_logger(), "Parameter set access rights for parameter %s couldn't be determined", param.getName().c_str()); return false; } @@ -72,13 +74,13 @@ bool ParameterHandler::canSetParameter(const ParameterBase & param) const } void ParameterHandler::registerParameter( - std::shared_ptr<ParameterBase> param_shared_ptr, - bool block) + std::shared_ptr<ParameterBase> param_shared_ptr, bool block) { params_.emplace_back(param_shared_ptr); param_shared_ptr->getParameterInterface()->declare_parameter( param_shared_ptr->getName(), param_shared_ptr->getDefaultValue()); - if (block) { + if (block) + { param_shared_ptr->blockParameter(); } } diff --git a/kuka_drivers_core/src/ros2_base_lc_node.cpp b/kuka_drivers_core/src/ros2_base_lc_node.cpp index 92728252..54af3fcf 100644 --- a/kuka_drivers_core/src/ros2_base_lc_node.cpp +++ b/kuka_drivers_core/src/ros2_base_lc_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include <memory> #include <string> #include <vector> -#include <memory> #include "kuka_drivers_core/ros2_base_lc_node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -26,10 +26,9 @@ ROS2BaseLCNode::ROS2BaseLCNode(const std::string & node_name, const rclcpp::Node : rclcpp_lifecycle::LifecycleNode(node_name, options) { param_handler_ = ParameterHandler(this); - param_callback_ = this->add_on_set_parameters_callback( - [this](const std::vector<rclcpp::Parameter> & parameters) { - return param_handler_.onParamChange(parameters); - }); + param_callback_ = + this->add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) + { return param_handler_.onParamChange(parameters); }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -48,10 +47,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ROS2BaseLCNode::on_shutdown(const rclcpp_lifecycle::State & state) { auto result = SUCCESS; - switch (state.id()) { + switch (state.id()) + { case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: result = this->on_deactivate(get_current_state()); - if (result != SUCCESS) { + if (result != SUCCESS) + { break; } result = this->on_cleanup(get_current_state()); @@ -79,20 +80,17 @@ ROS2BaseLCNode::on_deactivate(const rclcpp_lifecycle::State &) return SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -ROS2BaseLCNode::on_error(const rclcpp_lifecycle::State &) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ROS2BaseLCNode::on_error( + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(get_logger(), "An error occured"); + RCLCPP_INFO(get_logger(), "An error occurred"); return SUCCESS; } -const ParameterHandler & ROS2BaseLCNode::getParameterHandler() const -{ - return param_handler_; -} +const ParameterHandler & ROS2BaseLCNode::getParameterHandler() const { return param_handler_; } rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr ROS2BaseLCNode::ParamCallback() -const + const { return param_callback_; } diff --git a/kuka_drivers_core/src/ros2_base_node.cpp b/kuka_drivers_core/src/ros2_base_node.cpp index e0788a3a..dde6ccc0 100644 --- a/kuka_drivers_core/src/ros2_base_node.cpp +++ b/kuka_drivers_core/src/ros2_base_node.cpp @@ -12,32 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include <memory> #include <string> #include <vector> -#include <memory> #include "kuka_drivers_core/ros2_base_node.hpp" - namespace kuka_drivers_core { ROS2BaseNode::ROS2BaseNode(const std::string & node_name, const rclcpp::NodeOptions & options) : rclcpp::Node(node_name, options) { - param_callback_ = this->add_on_set_parameters_callback( - [this](const std::vector<rclcpp::Parameter> & parameters) { - return param_handler_.onParamChange(parameters); - }); + param_callback_ = + this->add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) + { return param_handler_.onParamChange(parameters); }); } -const ParameterHandler & ROS2BaseNode::getParameterHandler() const -{ - return param_handler_; -} +const ParameterHandler & ROS2BaseNode::getParameterHandler() const { return param_handler_; } rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr ROS2BaseNode::ParamCallback() -const + const { return param_callback_; } diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index d13b57d3..5cd737c0 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -98,33 +98,8 @@ install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) - file(GLOB_RECURSE mock_headers - LIST_DIRECTORIES FALSE - RELATIVE "${PROJECT_SOURCE_DIR}" - include/mock/*) - - file(GLOB_RECURSE mock_src - LIST_DIRECTORIES FALSE - RELATIVE "${PROJECT_SOURCE_DIR}" - src/mock/*) - - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_pep257 REQUIRED) - find_package(ament_cmake_flake8 REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_xmllint REQUIRED) - - ament_copyright(--exclude ${mock_headers} ${mock_src}) - ament_cppcheck(--language=c++) - ament_pep257() - ament_flake8() - ament_cpplint() - ament_lint_cmake() - ament_uncrustify(--exclude ${mock_headers} ${mock_src}) - ament_xmllint() + find_package(launch_testing_ament_cmake) + add_launch_test(test/test_driver_startup.py) endif() ament_export_libraries( diff --git a/kuka_iiqka_eac_driver/README.md b/kuka_iiqka_eac_driver/README.md deleted file mode 100644 index 2651480f..00000000 --- a/kuka_iiqka_eac_driver/README.md +++ /dev/null @@ -1 +0,0 @@ -ROS2 ported HW interface for RoX \ No newline at end of file diff --git a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml index 838d627b..99873d72 100644 --- a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml @@ -1,6 +1,6 @@ effort_controller: ros__parameters: - joints: + joints: - joint_1 - joint_2 - joint_3 diff --git a/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml b/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml index 7a2a52bc..725bf41b 100644 --- a/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml @@ -1,9 +1,9 @@ joint_group_impedance_controller: ros__parameters: - joints: + joints: - joint_1 - joint_2 - joint_3 - joint_4 - joint_5 - - joint_6 \ No newline at end of file + - joint_6 diff --git a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml index f164cb1e..3f727ef4 100644 --- a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml @@ -1,6 +1,6 @@ joint_trajectory_controller: ros__parameters: - joints: + joints: - joint_1 - joint_2 - joint_3 diff --git a/kuka_iiqka_eac_driver/config/qos_profiles.yaml b/kuka_iiqka_eac_driver/config/qos_profiles.yaml index 8270aeb7..b6c9a6b5 100644 --- a/kuka_iiqka_eac_driver/config/qos_profiles.yaml +++ b/kuka_iiqka_eac_driver/config/qos_profiles.yaml @@ -2,4 +2,3 @@ rt_packet_loss_profile: consequent_lost_packets: 2 lost_packets_in_timeframe: 1 timeframe_ms: 5000 - diff --git a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml index b313a96f..eb1e7c14 100644 --- a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml @@ -13,5 +13,4 @@ control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index c5ca913f..ca5948a1 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -15,19 +15,19 @@ #ifndef KUKA_IIQKA_EAC_DRIVER__HARDWARE_INTERFACE_HPP_ #define KUKA_IIQKA_EAC_DRIVER__HARDWARE_INTERFACE_HPP_ -#include <vector> -#include <string> -#include <memory> #include <chrono> #include <cmath> +#include <memory> #include <mutex> +#include <string> #include <thread> +#include <vector> -#include "rclcpp/rclcpp.hpp" +#include "hardware_interface/system_interface.hpp" +#include "pluginlib/class_list_macros.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "pluginlib/class_list_macros.hpp" -#include "hardware_interface/system_interface.hpp" #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" #include "nanopb/kuka/core/motion/joint.pb.hh" @@ -48,8 +48,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface public: RCLCPP_SHARED_PTR_DEFINITIONS(KukaEACHardwareInterface) - KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) - override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn + on_init(const hardware_interface::HardwareInfo & info) override; KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override; @@ -57,22 +57,20 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override; - KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; - KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) override; - KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type read( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type + read(const rclcpp::Time & time, const rclcpp::Duration & period) override; - KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type write( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type + write(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl(); @@ -99,7 +97,7 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface std::thread observe_thread_; std::unique_ptr<os::core::udp::communication::Replier> udp_replier_; - std::chrono::milliseconds receive_timeout_ {100}; + std::chrono::milliseconds receive_timeout_{100}; uint8_t out_buff_arr_[1500]; diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index f95558f2..9ccc2ddb 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -15,17 +15,17 @@ #ifndef KUKA_IIQKA_EAC_DRIVER__ROBOT_MANAGER_NODE_HPP_ #define KUKA_IIQKA_EAC_DRIVER__ROBOT_MANAGER_NODE_HPP_ -#include <string> +#include <map> #include <memory> +#include <string> #include <vector> -#include <map> -#include "rclcpp/rclcpp.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" #include "rclcpp/client.hpp" +#include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/u_int32.hpp" -#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" -#include "controller_manager_msgs/srv/switch_controller.hpp" #include "kuka_drivers_core/controller_handler.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" @@ -40,17 +40,17 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode RobotManagerNode(); ~RobotManagerNode(); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &) override; private: void ObserveControl(); @@ -90,7 +90,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode static constexpr int IMPEDANCE_MODE_IF_SIZE = 2; }; -} // namespace kuka_eac - +} // namespace kuka_eac #endif // KUKA_IIQKA_EAC_DRIVER__ROBOT_MANAGER_NODE_HPP_ diff --git a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h b/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h index 8f2c00e2..81fac0a8 100644 --- a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h @@ -1244,7 +1244,7 @@ inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::timeout() cons return _internal_timeout(); } inline void OpenControlChannelRequest::_internal_set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value) { - + timeout_ = value; } inline void OpenControlChannelRequest::set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value) { @@ -1264,7 +1264,7 @@ inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::cycle_time() c return _internal_cycle_time(); } inline void OpenControlChannelRequest::_internal_set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value) { - + cycle_time_ = value; } inline void OpenControlChannelRequest::set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value) { @@ -1284,7 +1284,7 @@ inline ::kuka::motion::external::ExternalControlMode OpenControlChannelRequest:: return _internal_external_control_mode(); } inline void OpenControlChannelRequest::_internal_set_external_control_mode(::kuka::motion::external::ExternalControlMode value) { - + external_control_mode_ = value; } inline void OpenControlChannelRequest::set_external_control_mode(::kuka::motion::external::ExternalControlMode value) { @@ -1379,7 +1379,7 @@ inline ::kuka::ecs::v1::CommandEvent CommandState::event() const { return _internal_event(); } inline void CommandState::_internal_set_event(::kuka::ecs::v1::CommandEvent value) { - + event_ = value; } inline void CommandState::set_event(::kuka::ecs::v1::CommandEvent value) { @@ -1407,31 +1407,31 @@ inline const std::string& CommandState::_internal_message() const { return message_.Get(); } inline void CommandState::_internal_set_message(const std::string& value) { - + message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena()); } inline void CommandState::set_message(std::string&& value) { - + message_.Set( &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena()); // @@protoc_insertion_point(field_set_rvalue:kuka.ecs.v1.CommandState.message) } inline void CommandState::set_message(const char* value) { GOOGLE_DCHECK(value != nullptr); - + message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value), GetArena()); // @@protoc_insertion_point(field_set_char:kuka.ecs.v1.CommandState.message) } inline void CommandState::set_message(const char* value, size_t size) { - + message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string( reinterpret_cast<const char*>(value), size), GetArena()); // @@protoc_insertion_point(field_set_pointer:kuka.ecs.v1.CommandState.message) } inline std::string* CommandState::_internal_mutable_message() { - + return message_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena()); } inline std::string* CommandState::release_message() { @@ -1440,9 +1440,9 @@ inline std::string* CommandState::release_message() { } inline void CommandState::set_allocated_message(std::string* message) { if (message != nullptr) { - + } else { - + } message_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), message, GetArena()); @@ -1451,7 +1451,7 @@ inline void CommandState::set_allocated_message(std::string* message) { inline std::string* CommandState::unsafe_arena_release_message() { // @@protoc_insertion_point(field_unsafe_arena_release:kuka.ecs.v1.CommandState.message) GOOGLE_DCHECK(GetArena() != nullptr); - + return message_.UnsafeArenaRelease(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena()); } @@ -1459,9 +1459,9 @@ inline void CommandState::unsafe_arena_set_allocated_message( std::string* message) { GOOGLE_DCHECK(GetArena() != nullptr); if (message != nullptr) { - + } else { - + } message_.UnsafeArenaSetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), message, GetArena()); diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h b/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h index 77af93a3..d887e822 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h @@ -17,4 +17,4 @@ inline bool Decode(const uint8_t *, size_t , T &) { // only a mock return false; } -} // namespace nanopb \ No newline at end of file +} // namespace nanopb diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h index a296b337..a5805e1e 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h @@ -7,9 +7,9 @@ #endif /* An array of joint positions for a given kinematic structure. */ -typedef struct _kuka_core_motion_JointPositions { +typedef struct _kuka_core_motion_JointPositions { pb_size_t values_count; - double values[24]; + double values[24]; } kuka_core_motion_JointPositions; @@ -29,4 +29,3 @@ extern "C" { #endif #endif - diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh index c0bd4d2e..95b38f29 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh @@ -14,4 +14,3 @@ constexpr JointPositions JointPositions_init_default kuka_core_motion_JointPosit } // namespace nanopb::kuka::core::motion #endif // PB_KUKA_CORE_MOTION_JOINT_PB_HH_INCLUDED - diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h index 37a6691f..72bf2ba9 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h @@ -8,7 +8,7 @@ #error Regenerate this file with the current version of nanopb generator. #endif -typedef struct _kuka_ecs_v1_ControlSignalExternal { +typedef struct _kuka_ecs_v1_ControlSignalExternal { bool has_header; kuka_ecs_v1_ExternalHeader header; diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h index 9ddd834e..f8ba8acc 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h @@ -7,8 +7,8 @@ #endif typedef struct _kuka_ecs_v1_ExternalHeader { - uint32_t message_id; - uint32_t ipoc; + uint32_t message_id; + uint32_t ipoc; } kuka_ecs_v1_ExternalHeader; diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h index 4a218f5e..f8ee2087 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h @@ -6,7 +6,7 @@ #error Regenerate this file with the current version of nanopb generator. #endif -typedef struct _kuka_motion_external_CartesianImpedanceControlAttributes { +typedef struct _kuka_motion_external_CartesianImpedanceControlAttributes { pb_size_t stiffness_count; double stiffness[24]; @@ -20,7 +20,7 @@ typedef struct _kuka_motion_external_CartesianImpedanceControlAttributes { double nullspace_damping[24]; } kuka_motion_external_CartesianImpedanceControlAttributes; -typedef struct _kuka_motion_external_JointImpedanceControlAttributes { +typedef struct _kuka_motion_external_JointImpedanceControlAttributes { pb_size_t stiffness_count; double stiffness[24]; diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h index 95f23357..9982aae1 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h @@ -11,9 +11,9 @@ #endif typedef struct _kuka_core_geometry_Vector { - double x; - double y; - double z; + double x; + double y; + double z; } kuka_core_geometry_Vector; #define kuka_core_geometry_Vector_init_default {0, 0, 0} @@ -21,31 +21,31 @@ typedef struct _kuka_core_geometry_Vector { #define kuka_core_geometry_Vector_size 27 typedef struct _kuka_core_geometry_Quaternion { - double qx; - double qy; - double qz; - double qw; + double qx; + double qy; + double qz; + double qw; } kuka_core_geometry_Quaternion; #define kuka_core_geometry_Quaternion_init_default {0, 0, 0, 0} #define kuka_core_geometry_Quaternion_init_zero {0, 0, 0, 0} #define kuka_core_geometry_Quaternion_size 36 -typedef struct _kuka_core_geometry_Transform { +typedef struct _kuka_core_geometry_Transform { bool has_translation; - kuka_core_geometry_Vector translation; + kuka_core_geometry_Vector translation; bool has_rotation; - kuka_core_geometry_Quaternion rotation; + kuka_core_geometry_Quaternion rotation; } kuka_core_geometry_Transform; #define kuka_core_geometry_Transform_init_default {false, kuka_core_geometry_Vector_init_default, false, kuka_core_geometry_Quaternion_init_default} #define kuka_core_geometry_Transform_init_zero {false, kuka_core_geometry_Vector_init_zero, false, kuka_core_geometry_Quaternion_init_zero} -typedef struct _kuka_core_motion_Twist { +typedef struct _kuka_core_motion_Twist { bool has_linear; - kuka_core_geometry_Vector linear; + kuka_core_geometry_Vector linear; bool has_angular; - kuka_core_geometry_Vector angular; + kuka_core_geometry_Vector angular; } kuka_core_motion_Twist; #define kuka_core_motion_Twist_init_default {false, kuka_core_geometry_Vector_init_default, false, kuka_core_geometry_Vector_init_default} @@ -53,34 +53,34 @@ typedef struct _kuka_core_motion_Twist { #define kuka_core_motion_Twist_size 58 -typedef struct _kuka_motion_external_ControlSignalInternal { - bool stop_ipo; +typedef struct _kuka_motion_external_ControlSignalInternal { + bool stop_ipo; bool has_joint_command; - kuka_core_motion_JointPositions joint_command; + kuka_core_motion_JointPositions joint_command; bool has_cartesian_command; - kuka_core_geometry_Transform cartesian_command; + kuka_core_geometry_Transform cartesian_command; bool has_joint_velocity_command; - kuka_core_motion_JointPositions joint_velocity_command; - + kuka_core_motion_JointPositions joint_velocity_command; + bool has_twist_command; - kuka_core_motion_Twist twist_command; + kuka_core_motion_Twist twist_command; bool has_joint_torque_command; - kuka_core_motion_JointPositions joint_torque_command; + kuka_core_motion_JointPositions joint_torque_command; bool has_wrench_command; - kuka_core_motion_JointPositions wrench_command; + kuka_core_motion_JointPositions wrench_command; bool has_joint_attributes; - kuka_motion_external_JointImpedanceControlAttributes joint_attributes; + kuka_motion_external_JointImpedanceControlAttributes joint_attributes; bool has_cartesian_attributes; - kuka_motion_external_CartesianImpedanceControlAttributes cartesian_attributes; + kuka_motion_external_CartesianImpedanceControlAttributes cartesian_attributes; - kuka_motion_external_ExternalControlMode control_mode; + kuka_motion_external_ExternalControlMode control_mode; } kuka_motion_external_ControlSignalInternal; diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h index 3cd67cd4..843437e0 100644 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h +++ b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h @@ -8,19 +8,19 @@ #error Regenerate this file with the current version of nanopb generator. #endif -typedef struct _kuka_motion_external_MotionStateInternal { - bool ipo_stopped; +typedef struct _kuka_motion_external_MotionStateInternal { + bool ipo_stopped; - kuka_motion_external_ExternalControlMode control_mode; + kuka_motion_external_ExternalControlMode control_mode; bool has_measured_positions; - kuka_core_motion_JointPositions measured_positions; + kuka_core_motion_JointPositions measured_positions; bool has_measured_velocities; - kuka_core_motion_JointPositions measured_velocities; + kuka_core_motion_JointPositions measured_velocities; bool has_measured_torques; - kuka_core_motion_JointPositions measured_torques; + kuka_core_motion_JointPositions measured_torques; } kuka_motion_external_MotionStateInternal; diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index a591a906..59191314 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -22,7 +22,7 @@ def launch_setup(context, *args, **kwargs): - robot_model = LaunchConfiguration('robot_model') + robot_model = LaunchConfiguration("robot_model") # Get URDF via xacro robot_description_content = Command( @@ -30,48 +30,61 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare("kuka_lbr_iisy_support"), - "urdf", robot_model.perform(context) + ".urdf.xacro"] + [ + FindPackageShare("kuka_lbr_iisy_support"), + "urdf", + robot_model.perform(context) + ".urdf.xacro", + ] ), " ", - ], on_stderr='capture' + ], + on_stderr="capture", ) # Get URDF via xacro - robot_description = {'robot_description': robot_description_content} + robot_description = {"robot_description": robot_description_content} - controller_config = (get_package_share_directory('kuka_iiqka_eac_driver') + - "/config/ros2_controller_config.yaml") + controller_config = ( + get_package_share_directory("kuka_iiqka_eac_driver") + + "/config/ros2_controller_config.yaml" + ) - joint_traj_controller_config = (get_package_share_directory('kuka_iiqka_eac_driver') + - "/config/joint_trajectory_controller_config.yaml") - effort_controller_config = (get_package_share_directory('kuka_iiqka_eac_driver') + - "/config/effort_controller_config.yaml") - joint_imp_controller_config = (get_package_share_directory('kuka_iiqka_eac_driver') + - "/config/joint_impedance_controller_config.yaml") + joint_traj_controller_config = ( + get_package_share_directory("kuka_iiqka_eac_driver") + + "/config/joint_trajectory_controller_config.yaml" + ) + effort_controller_config = ( + get_package_share_directory("kuka_iiqka_eac_driver") + + "/config/effort_controller_config.yaml" + ) + joint_imp_controller_config = ( + get_package_share_directory("kuka_iiqka_eac_driver") + + "/config/joint_impedance_controller_config.yaml" + ) - driver_config = (get_package_share_directory( - 'kuka_iiqka_eac_driver') + "/config/driver_config.yaml") + driver_config = ( + get_package_share_directory("kuka_iiqka_eac_driver") + "/config/driver_config.yaml" + ) - controller_manager_node = '/controller_manager' + controller_manager_node = "/controller_manager" control_node = Node( - package='kuka_drivers_core', - executable='control_node', - parameters=[robot_description, controller_config] + package="kuka_drivers_core", + executable="control_node", + parameters=[robot_description, controller_config], ) robot_manager_node = LifecycleNode( - name=['robot_manager'], - namespace='', + name=["robot_manager"], + namespace="", package="kuka_iiqka_eac_driver", executable="robot_manager_node", - parameters=[driver_config, {'robot_model': robot_model.perform(context)}] + parameters=[driver_config, {"robot_model": robot_model.perform(context)}], ) robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], ) # Spawn controllers @@ -79,8 +92,14 @@ def controller_spawner(controller_with_config): return Node( package="controller_manager", executable="spawner", - arguments=[controller_with_config[0], "-c", controller_manager_node, "-p", - controller_with_config[1], "--inactive"] + arguments=[ + controller_with_config[0], + "-c", + controller_manager_node, + "-p", + controller_with_config[1], + "--inactive", + ], ) controller_names_and_config = [ @@ -88,16 +107,17 @@ def controller_spawner(controller_with_config): ("joint_trajectory_controller", joint_traj_controller_config), ("joint_group_impedance_controller", joint_imp_controller_config), ("effort_controller", effort_controller_config), - ("control_mode_handler", []) + ("control_mode_handler", []), ] - controller_spawners = [controller_spawner(controllers) - for controllers in controller_names_and_config] + controller_spawners = [ + controller_spawner(controllers) for controllers in controller_names_and_config + ] nodes_to_start = [ control_node, robot_manager_node, - robot_state_publisher + robot_state_publisher, ] + controller_spawners return nodes_to_start @@ -105,8 +125,5 @@ def controller_spawner(controller_with_config): def generate_launch_description(): launch_arguments = [] - launch_arguments.append(DeclareLaunchArgument( - 'robot_model', - default_value='lbr_iisy3_r760' - )) + launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760")) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py b/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py index cf37359b..1f292616 100644 --- a/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py @@ -19,24 +19,31 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch.actions.include_launch_description import IncludeLaunchDescription -from launch.launch_description_sources.python_launch_description_source import PythonLaunchDescriptionSource # noqa: E501 +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 def generate_launch_description(): rviz_config_file = os.path.join( - get_package_share_directory('kuka_resources'), 'config', 'view_6_axis_urdf.rviz') + get_package_share_directory("kuka_resources"), "config", "view_6_axis_urdf.rviz" + ) - startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( - [get_package_share_directory('kuka_iiqka_eac_driver'), '/launch/startup.launch.py'])) - - return LaunchDescription([ - startup_launch, - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file, - "--ros-args", "--log-level", "error"], + startup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory("kuka_iiqka_eac_driver"), "/launch/startup.launch.py"] ) - ]) + ) + + return LaunchDescription( + [ + startup_launch, + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, "--ros-args", "--log-level", "error"], + ), + ] + ) diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index b0fbdc80..b97214a3 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -3,7 +3,7 @@ <package format="3"> <name>kuka_iiqka_eac_driver</name> <version>0.0.0</version> - <description>A ROS2 hardware interface for use with KUKA RoX</description> + <description>A ROS2 hardware interface for use with KUKA iiQKA OS</description> <maintainer email="svastits1@gmail.com">Aron Svastits</maintainer> <license>Apache 2.0</license> @@ -16,11 +16,10 @@ <depend>std_srvs</depend> <depend>sensor_msgs</depend> <depend>kuka_drivers_core</depend> - <depend>kuka_sunrise_fri_driver</depend> <depend>tinyxml_vendor</depend> <depend>hardware_interface</depend> <depend>pluginlib</depend> - + <depend>yaml-cpp</depend> <depend>protobuf-dev</depend> <depend>libnanopb-dev</depend> @@ -30,14 +29,6 @@ <exec_depend>ros2_control</exec_depend> <exec_depend>ros2_controllers</exec_depend> - <test_depend>ament_cmake_copyright</test_depend> - <test_depend>ament_cmake_cppcheck</test_depend> - <test_depend>ament_cmake_pep257</test_depend> - <test_depend>ament_cmake_flake8</test_depend> - <test_depend>ament_cmake_cpplint</test_depend> - <test_depend>ament_cmake_lint_cmake</test_depend> - <test_depend>ament_cmake_xmllint</test_depend> - <export> <build_type>ament_cmake</build_type> </export> diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 75a5099c..0b6208a5 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -32,21 +32,18 @@ namespace kuka_eac { CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } udp_replier_ = std::make_unique<os::core::udp::communication::Replier>( - os::core::udp::communication::SocketAddress( - info_.hardware_parameters.at("client_ip"), 44444)); + os::core::udp::communication::SocketAddress(info_.hardware_parameters.at("client_ip"), 44444)); #ifdef NON_MOCK_SETUP - stub_ = - ExternalControlService::NewStub( - grpc::CreateChannel( - info_.hardware_parameters.at("controller_ip") + ":49335", - grpc::InsecureChannelCredentials())); + stub_ = ExternalControlService::NewStub(grpc::CreateChannel( + info_.hardware_parameters.at("controller_ip") + ":49335", grpc::InsecureChannelCredentials())); #endif hw_control_mode_command_ = std::stod(info_.hardware_parameters.at("control_mode")); @@ -67,99 +64,154 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw control_signal_ext_.control_signal.joint_attributes.stiffness_count = info_.joints.size(); control_signal_ext_.control_signal.joint_attributes.damping_count = info_.joints.size(); #ifdef NON_MOCK_SETUP - if (udp_replier_->Setup() != Socket::ErrorCode::kSuccess) { + if (udp_replier_->Setup() != Socket::ErrorCode::kSuccess) + { RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Could not setup udp replier"); return CallbackReturn::FAILURE; } #else -// Start from home position in mock mode + // Start from home position in mock mode hw_position_commands_[1] = -90 * (M_PI / 180); hw_position_commands_[2] = 90 * (M_PI / 180); hw_position_commands_[4] = 90 * (M_PI / 180); #endif + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != 4) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), "expecting exactly 4 command interface"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'POSITION' command interface as first"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[1].name != hardware_interface::HW_IF_STIFFNESS) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'STIFFNESS' command interface as second"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[2].name != hardware_interface::HW_IF_DAMPING) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'DAMPING' command interface as third"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[3].name != hardware_interface::HW_IF_EFFORT) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'EFFORT' command interface as fourth"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), "expecting exactly 2 state interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'POSITION' state interface as first"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_EFFORT) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaEACHardwareInterface"), + "expecting 'EFFORT' state interface as second"); + return CallbackReturn::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Init successful"); return CallbackReturn::SUCCESS; } -std::vector<hardware_interface::StateInterface> -KukaEACHardwareInterface::export_state_interfaces() +std::vector<hardware_interface::StateInterface> KukaEACHardwareInterface::export_state_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Export state interfaces"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaEACHardwareInterface"), "Export state interfaces"); std::vector<hardware_interface::StateInterface> state_interfaces; - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { state_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_position_states_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_states_[i]); state_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_EFFORT, - &hw_torque_states_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_states_[i]); } return state_interfaces; } -std::vector<hardware_interface::CommandInterface> KukaEACHardwareInterface:: -export_command_interfaces() +std::vector<hardware_interface::CommandInterface> +KukaEACHardwareInterface::export_command_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Export command interfaces"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaEACHardwareInterface"), "Export command interfaces"); std::vector<hardware_interface::CommandInterface> command_interfaces; - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { command_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_position_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_EFFORT, - &hw_torque_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_STIFFNESS, - &hw_stiffness_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_DAMPING, - &hw_damping_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[i]); } command_interfaces.emplace_back( - hardware_interface::CONFIG_PREFIX, - hardware_interface::CONTROL_MODE, - &hw_control_mode_command_); + hardware_interface::CONFIG_PREFIX, hardware_interface::CONTROL_MODE, &hw_control_mode_command_); return command_interfaces; } CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { - #ifdef NON_MOCK_SETUP +#ifdef NON_MOCK_SETUP SetQoSProfileRequest request; SetQoSProfileResponse response; grpc::ClientContext context; request.add_qos_profiles(); - request.mutable_qos_profiles()->at(0).mutable_rt_packet_loss_profile()-> - set_consequent_lost_packets(std::stoi(info_.hardware_parameters.at("consequent_lost_packets"))); - request.mutable_qos_profiles()->at(0).mutable_rt_packet_loss_profile()-> - set_lost_packets_in_timeframe( - std::stoi( - info_.hardware_parameters.at( - "lost_packets_in_timeframe"))); + request.mutable_qos_profiles() + ->at(0) + .mutable_rt_packet_loss_profile() + ->set_consequent_lost_packets( + std::stoi(info_.hardware_parameters.at("consequent_lost_packets"))); + request.mutable_qos_profiles() + ->at(0) + .mutable_rt_packet_loss_profile() + ->set_lost_packets_in_timeframe( + std::stoi(info_.hardware_parameters.at("lost_packets_in_timeframe"))); request.mutable_qos_profiles()->at(0).mutable_rt_packet_loss_profile()->set_timeframe_ms( - std::stoi( - info_.hardware_parameters.at( - "timeframe_ms"))); + std::stoi(info_.hardware_parameters.at("timeframe_ms"))); - if (stub_->SetQoSProfile(&context, request, &response).error_code() != grpc::StatusCode::OK) { + if (stub_->SetQoSProfile(&context, request, &response).error_code() != grpc::StatusCode::OK) + { RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "SetQoSProfile failed"); return CallbackReturn::FAILURE; } @@ -170,11 +222,10 @@ CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::St info_.hardware_parameters.at("consequent_lost_packets").c_str(), info_.hardware_parameters.at("lost_packets_in_timeframe").c_str(), info_.hardware_parameters.at("timeframe_ms").c_str()); - #endif +#endif return CallbackReturn::SUCCESS; } - CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Connecting to robot . . ."); @@ -182,11 +233,13 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta receive_timeout_ = std::chrono::milliseconds(100); control_signal_ext_.control_signal.stop_ipo = false; #ifdef NON_MOCK_SETUP - if (context_ != nullptr) { + if (context_ != nullptr) + { context_->TryCancel(); } #endif - if (observe_thread_.joinable()) { + if (observe_thread_.joinable()) + { observe_thread_.join(); } @@ -204,11 +257,12 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta kuka::motion::external::ExternalControlMode(hw_control_mode_command_)); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Starting control in %s", - kuka::motion::external::ExternalControlMode_Name( - static_cast<int>(hw_control_mode_command_)).c_str()); + kuka::motion::external::ExternalControlMode_Name(static_cast<int>(hw_control_mode_command_)) + .c_str()); auto ret = stub_->OpenControlChannel(&context, request, &response); - if (ret.error_code() != grpc::StatusCode::OK) { + if (ret.error_code() != grpc::StatusCode::OK) + { RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "%s", ret.error_message().c_str()); return CallbackReturn::FAILURE; } @@ -217,121 +271,125 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta return CallbackReturn::SUCCESS; } -CallbackReturn KukaEACHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State &) +CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating"); control_signal_ext_.control_signal.stop_ipo = true; - while (is_active_) { + while (is_active_) + { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } #ifdef NON_MOCK_SETUP - if (context_ != nullptr) { + if (context_ != nullptr) + { context_->TryCancel(); } #endif - if (observe_thread_.joinable()) { + if (observe_thread_.joinable()) + { observe_thread_.join(); } return CallbackReturn::SUCCESS; } -return_type KukaEACHardwareInterface::read( - const rclcpp::Time &, - const rclcpp::Duration &) +return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { #ifndef NON_MOCK_SETUP std::this_thread::sleep_for(std::chrono::microseconds(3900)); - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { hw_position_states_[i] = hw_position_commands_[i]; } return return_type::OK; #endif - if (!is_active_) { + if (!is_active_) + { std::this_thread::sleep_for(std::chrono::milliseconds(2)); msg_received_ = false; return return_type::OK; } - if (udp_replier_->ReceiveRequestOrTimeout(receive_timeout_) == - Socket::ErrorCode::kSuccess) + if (udp_replier_->ReceiveRequestOrTimeout(receive_timeout_) == Socket::ErrorCode::kSuccess) { auto req_message = udp_replier_->GetRequestMessage(); if (!nanopb::Decode<nanopb::kuka::ecs::v1::MotionStateExternal>( - req_message.first, req_message.second, motion_state_external_)) + req_message.first, req_message.second, motion_state_external_)) { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Decoding request failed"); throw std::runtime_error("Decoding request failed"); } control_signal_ext_.header.ipoc = motion_state_external_.header.ipoc; - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { hw_position_states_[i] = motion_state_external_.motion_state.measured_positions.values[i]; hw_torque_states_[i] = motion_state_external_.motion_state.measured_torques.values[i]; // This is necessary, as joint trajectory controller is initialized with 0 command values - if (!msg_received_ && motion_state_external_.header.ipoc == 0) { + if (!msg_received_ && motion_state_external_.header.ipoc == 0) + { hw_position_commands_[i] = hw_position_states_[i]; } } - if (motion_state_external_.motion_state.ipo_stopped) { + if (motion_state_external_.motion_state.ipo_stopped) + { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Motion stopped"); } msg_received_ = true; receive_timeout_ = std::chrono::milliseconds(6); - } else { + } + else + { RCLCPP_WARN(rclcpp::get_logger("KukaEACHardwareInterface"), "Request was missed"); RCLCPP_WARN( - rclcpp::get_logger("KukaEACHardwareInterface"), - "Previous ipoc: %i", motion_state_external_.header.ipoc); + rclcpp::get_logger("KukaEACHardwareInterface"), "Previous ipoc: %i", + motion_state_external_.header.ipoc); msg_received_ = false; } return return_type::OK; } -return_type KukaEACHardwareInterface::write( - const rclcpp::Time &, - const rclcpp::Duration &) +return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) { // If control is not started or a request is missed, do not send back anything - if (!msg_received_) { + if (!msg_received_) + { return return_type::OK; } // TODO(Svastits): should we separate control modes somehow? std::copy( - hw_position_commands_.begin(), - hw_position_commands_.end(), control_signal_ext_.control_signal.joint_command.values); + hw_position_commands_.begin(), hw_position_commands_.end(), + control_signal_ext_.control_signal.joint_command.values); std::copy( - hw_torque_commands_.begin(), - hw_torque_commands_.end(), control_signal_ext_.control_signal.joint_torque_command.values); + hw_torque_commands_.begin(), hw_torque_commands_.end(), + control_signal_ext_.control_signal.joint_torque_command.values); std::copy( - hw_stiffness_commands_.begin(), - hw_stiffness_commands_.end(), control_signal_ext_.control_signal.joint_attributes.stiffness); + hw_stiffness_commands_.begin(), hw_stiffness_commands_.end(), + control_signal_ext_.control_signal.joint_attributes.stiffness); std::copy( - hw_damping_commands_.begin(), - hw_damping_commands_.end(), control_signal_ext_.control_signal.joint_attributes.damping); + hw_damping_commands_.begin(), hw_damping_commands_.end(), + control_signal_ext_.control_signal.joint_attributes.damping); - control_signal_ext_.control_signal.control_mode = kuka_motion_external_ExternalControlMode( - static_cast<int>(hw_control_mode_command_)); + control_signal_ext_.control_signal.control_mode = + kuka_motion_external_ExternalControlMode(static_cast<int>(hw_control_mode_command_)); auto encoded_bytes = nanopb::Encode<nanopb::kuka::ecs::v1::ControlSignalExternal>( control_signal_ext_, out_buff_arr_, sizeof(out_buff_arr_)); - if (encoded_bytes < 0) { + if (encoded_bytes < 0) + { RCLCPP_ERROR( - rclcpp::get_logger( - "KukaEACHardwareInterface"), + rclcpp::get_logger("KukaEACHardwareInterface"), "Encoding of control signal to out_buffer failed."); throw std::runtime_error("Encoding of control signal to out_buffer failed."); } - if (udp_replier_->SendReply(out_buff_arr_, encoded_bytes) != - Socket::ErrorCode::kSuccess) + if (udp_replier_->SendReply(out_buff_arr_, encoded_bytes) != Socket::ErrorCode::kSuccess) { RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Error sending reply"); throw std::runtime_error("Error sending reply"); @@ -350,15 +408,16 @@ void KukaEACHardwareInterface::ObserveControl() CommandState response; - while (reader->Read(&response)) { + while (reader->Read(&response)) + { command_state_ = response; RCLCPP_INFO( - rclcpp::get_logger( - "KukaEACHardwareInterface"), - "Event streamed from external control service: %s", CommandEvent_Name( - response.event()).c_str()); + rclcpp::get_logger("KukaEACHardwareInterface"), + "Event streamed from external control service: %s", + CommandEvent_Name(response.event()).c_str()); - switch (static_cast<int>(response.event())) { + switch (static_cast<int>(response.event())) + { case CommandEvent::COMMAND_READY: RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Command accepted"); break; @@ -368,8 +427,7 @@ void KukaEACHardwareInterface::ObserveControl() break; case CommandEvent::CONTROL_MODE_SWITCH: RCLCPP_INFO( - rclcpp::get_logger( - "KukaEACHardwareInterface"), "Control mode switch is in progress"); + rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); is_active_ = false; break; case CommandEvent::STOPPED: @@ -378,9 +436,7 @@ void KukaEACHardwareInterface::ObserveControl() break; case CommandEvent::ERROR: RCLCPP_ERROR( - rclcpp::get_logger( - "KukaEACHardwareInterface"), - "External control stopped by an error"); + rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), response.message().c_str()); is_active_ = false; break; @@ -391,8 +447,6 @@ void KukaEACHardwareInterface::ObserveControl() #endif } -} // namespace namespace kuka_eac +} // namespace kuka_eac -PLUGINLIB_EXPORT_CLASS( - kuka_eac::KukaEACHardwareInterface, - hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(kuka_eac::KukaEACHardwareInterface, hardware_interface::SystemInterface) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 41fa1190..975ef3a6 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -20,106 +20,101 @@ #include "kuka_iiqka_eac_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT -using namespace lifecycle_msgs::msg; // NOLINT -using namespace kuka::motion::external; // NOLINT +using namespace lifecycle_msgs::msg; // NOLINT +using namespace kuka::motion::external; // NOLINT namespace kuka_eac { -// TODO(Komaromi): Readd "control_mode_handler" controller to controller_handlers constrctor -// after controller handler poperly implemented with working initial control mode change +// TODO(Komaromi): Re-add "control_mode_handler" controller to controller_handlers constructor +// after controller handler properly implemented with working initial control mode change RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager"), - controller_handler_({"joint_state_broadcaster", }) + controller_handler_({ + "joint_state_broadcaster", + }) #ifdef NON_MOCK_SETUP - , control_mode_change_finished_(false) + , + control_mode_change_finished_(false) #endif { - RCLCPP_INFO(get_logger(), "Starting Robot Manager Node init"); + RCLCPP_DEBUG(get_logger(), "Starting Robot Manager Node init"); auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - change_hardware_state_client_ = - this->create_client<SetHardwareComponentState>( - "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_ - ); - change_controller_state_client_ = - this->create_client<SwitchController>( - "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_ - ); + change_hardware_state_client_ = this->create_client<SetHardwareComponentState>( + "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); + change_controller_state_client_ = this->create_client<SwitchController>( + "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_); auto is_configured_qos = rclcpp::QoS(rclcpp::KeepLast(1)); is_configured_qos.best_effort(); - is_configured_pub_ = this->create_publisher<std_msgs::msg::Bool>( - "robot_manager/is_configured", - is_configured_qos); + is_configured_pub_ = + this->create_publisher<std_msgs::msg::Bool>("robot_manager/is_configured", is_configured_qos); control_mode_pub_ = this->create_publisher<std_msgs::msg::UInt32>( - "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS() - ); + "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); // Register parameters this->registerParameter<std::string>( - "position_controller_name", "", kuka_drivers_core::ParameterSetAccessRights {true, true, - false, false, false}, [this](const std::string & controller_name) { + "position_controller_name", "", + kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + [this](const std::string & controller_name) + { return this->controller_handler_.UpdateControllerName( - kuka_drivers_core::ControllerType::JOINT_POSITION_CONTROLLER_TYPE, - controller_name); + kuka_drivers_core::ControllerType::JOINT_POSITION_CONTROLLER_TYPE, controller_name); }); this->registerParameter<std::string>( - "impedance_controller_name", "", kuka_drivers_core::ParameterSetAccessRights {true, true, - false, false, false}, [this](const std::string & controller_name) { + "impedance_controller_name", "", + kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + [this](const std::string & controller_name) + { return this->controller_handler_.UpdateControllerName( - kuka_drivers_core::ControllerType::JOINT_IMPEDANCE_CONTROLLER_TYPE, - controller_name); + kuka_drivers_core::ControllerType::JOINT_IMPEDANCE_CONTROLLER_TYPE, controller_name); }); this->registerParameter<std::string>( - "torque_controller_name", "", kuka_drivers_core::ParameterSetAccessRights {true, true, false, - false, false}, [this](const std::string & controller_name) { + "torque_controller_name", "", + kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + [this](const std::string & controller_name) + { return this->controller_handler_.UpdateControllerName( - kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, - controller_name); + kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); this->registerParameter<int>( "control_mode", static_cast<int>(ExternalControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, true, - true, false, false}, [this](int control_mode) { - return this->onControlModeChangeRequest(control_mode); - }); + kuka_drivers_core::ParameterSetAccessRights{true, true, true, false, false}, + [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter<std::string>( - "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights {true, false, false, - false, false}, [this](const std::string &) { - return true; - }); + "controller_ip", "", + kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + [this](const std::string &) { return true; }); this->registerStaticParameter<std::string>( "robot_model", "lbr_iisy3_r760", - kuka_drivers_core::ParameterSetAccessRights{true, false, - false, false, false}, [this](const std::string & robot_model) { - return this->onRobotModelChangeRequest(robot_model); - }); - + kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + [this](const std::string & robot_model) + { return this->onRobotModelChangeRequest(robot_model); }); #ifdef NON_MOCK_SETUP RCLCPP_INFO( - get_logger(), "IP address of controller: %s", this->get_parameter( - "controller_ip").as_string().c_str()); - - stub_ = - kuka::ecs::v1::ExternalControlService::NewStub( - grpc::CreateChannel( - this->get_parameter("controller_ip").as_string() + ":49335", - grpc::InsecureChannelCredentials())); + get_logger(), "IP address of controller: %s", + this->get_parameter("controller_ip").as_string().c_str()); + + stub_ = kuka::ecs::v1::ExternalControlService::NewStub(grpc::CreateChannel( + this->get_parameter("controller_ip").as_string() + ":49335", + grpc::InsecureChannelCredentials())); #endif } RobotManagerNode::~RobotManagerNode() { #ifdef NON_MOCK_SETUP - if (context_ != nullptr) { + if (context_ != nullptr) + { context_->TryCancel(); } #endif - if (observe_thread_.joinable()) { + if (observe_thread_.joinable()) + { observe_thread_.join(); } } @@ -127,15 +122,14 @@ RobotManagerNode::~RobotManagerNode() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { - // Publish control mode paramater + // Publish control mode parameter auto message = std_msgs::msg::UInt32(); message.data = static_cast<uint32_t>(this->get_parameter("control_mode").as_int()); control_mode_pub_->publish(message); // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_INACTIVE)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; @@ -147,7 +141,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Activate control mode handler if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"control_mode_handler"}, {})) + change_controller_state_client_, {"control_mode_handler"}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate control mode handler"); // TODO(Svastits): this can be removed if rollback is implemented properly @@ -165,22 +159,21 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Deactivate control mode handler if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - {"control_mode_handler"})) + change_controller_state_client_, {}, {"control_mode_handler"})) { RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler"); } // Clean up hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_UNCONFIGURED)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_UNCONFIGURED)) { RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } - if (is_configured_pub_->is_activated()) { + if (is_configured_pub_->is_activated()) + { is_configured_msg_.data = false; is_configured_pub_->publish(is_configured_msg_); is_configured_pub_->on_deactivate(); @@ -191,7 +184,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) void RobotManagerNode::ObserveControl() { - #ifdef NON_MOCK_SETUP +#ifdef NON_MOCK_SETUP context_ = std::make_unique<::grpc::ClientContext>(); kuka::ecs::v1::ObserveControlStateRequest observe_request; std::unique_ptr<grpc::ClientReader<kuka::ecs::v1::CommandState>> reader( @@ -199,13 +192,15 @@ void RobotManagerNode::ObserveControl() kuka::ecs::v1::CommandState response; - while (reader->Read(&response)) { - switch (static_cast<int>(response.event())) { + while (reader->Read(&response)) + { + switch (static_cast<int>(response.event())) + { case kuka::ecs::v1::CommandEvent::CONTROL_MODE_SWITCH: - { - std::lock_guard<std::mutex> lk(control_mode_cv_m_); - control_mode_change_finished_ = true; - } + { + std::lock_guard<std::mutex> lk(control_mode_cv_m_); + control_mode_change_finished_ = true; + } RCLCPP_INFO(get_logger(), "Command mode switched in the robot controller"); control_mode_cv_.notify_all(); break; @@ -213,9 +208,12 @@ void RobotManagerNode::ObserveControl() case kuka::ecs::v1::CommandEvent::ERROR: RCLCPP_INFO(get_logger(), "External control stopped"); terminate_ = true; - if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) { + if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) + { this->deactivate(); - } else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) { + } + else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) + { // TODO(Svastits): this can be removed if rollback is implemented properly this->on_deactivate(get_current_state()); } @@ -232,12 +230,14 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { #ifdef NON_MOCK_SETUP - if (context_ != nullptr) { + if (context_ != nullptr) + { context_->TryCancel(); } #endif // Join observe thread, necessary if previous activation failed - if (observe_thread_.joinable()) { + if (observe_thread_.joinable()) + { observe_thread_.join(); } terminate_ = false; @@ -246,8 +246,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_ACTIVE, 5000)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_ACTIVE, 5000)) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); return FAILURE; @@ -257,16 +256,20 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) auto control_mode = this->get_parameter("control_mode").as_int(); std::pair<std::vector<std::string>, std::vector<std::string>> new_controllers; - try { - new_controllers = controller_handler_.GetControllersForSwitch( - kuka_drivers_core::ControlMode(control_mode)); - } catch (const std::exception & e) { + try + { + new_controllers = + controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); + } + catch (const std::exception & e) + { RCLCPP_ERROR(get_logger(), "Error while activating controllers: %s", e.what()); return ERROR; } // Deactivate list for activation should always be empty, safety check - if (!new_controllers.second.empty()) { + if (!new_controllers.second.empty()) + { RCLCPP_ERROR( get_logger(), "Controller handler state is improper, active controller list not empty before activation"); @@ -275,8 +278,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate RT controller(s) if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, new_controllers.first, - new_controllers.second)) + change_controller_state_client_, new_controllers.first, new_controllers.second)) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); @@ -284,7 +286,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) } controller_handler_.ApproveControllerActivation(); - if (!controller_handler_.ApproveControllerDeactivation()) { + if (!controller_handler_.ApproveControllerDeactivation()) + { RCLCPP_ERROR( get_logger(), "Controller handler state is improper, active controller list was modified before approval"); @@ -292,9 +295,9 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Successfully activated controllers"); - // Return failure if control is stopped while in state activating - if (terminate_) { + if (terminate_) + { RCLCPP_ERROR(get_logger(), "UDP communication could not be set up"); return FAILURE; } @@ -308,8 +311,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Deactivate hardware interface // Deactivation was not stable with 2000 ms timeout if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_INACTIVE, 3000)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_INACTIVE, 3000)) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; @@ -317,14 +319,15 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - controller_handler_.GetControllersForDeactivation(), SwitchController::Request::BEST_EFFORT)) + change_controller_state_client_, {}, controller_handler_.GetControllersForDeactivation(), + SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop RT controllers"); return ERROR; } - if (!controller_handler_.ApproveControllerDeactivation()) { + if (!controller_handler_.ApproveControllerDeactivation()) + { RCLCPP_ERROR( get_logger(), "Controller handler state is improper, active controller list was modified before approval"); @@ -336,17 +339,17 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { - if (param_declared_ && this->get_parameter("control_mode").as_int() == control_mode) { + if (param_declared_ && this->get_parameter("control_mode").as_int() == control_mode) + { RCLCPP_WARN(get_logger(), "Tried to change control mode to the one currently used"); return true; } RCLCPP_INFO(get_logger(), "Control mode change requested"); // TODO(komaromi): Remove this if a new control mode is supported - if (control_mode == - static_cast<int>(kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL) || - control_mode == - static_cast<int>(kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL) || + if ( + control_mode == static_cast<int>(kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL) || + control_mode == static_cast<int>(kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL) || control_mode == static_cast<int>(kuka_drivers_core::ControlMode::WRENCH_CONTROL) || control_mode == static_cast<int>(kuka_drivers_core::ControlMode::JOINT_VELOCITY_CONTROL) || control_mode == static_cast<int>(kuka_drivers_core::ControlMode::CARTESIAN_VELOCITY_CONTROL)) @@ -357,21 +360,27 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) std::pair<std::vector<std::string>, std::vector<std::string>> switch_controllers; - bool is_active_state = get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + bool is_active_state = + get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; // Determine which controllers to activate and deactivate - try { - switch_controllers = controller_handler_.GetControllersForSwitch( - kuka_drivers_core::ControlMode(control_mode)); - } catch (const std::exception & e) { + try + { + switch_controllers = + controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); + } + catch (const std::exception & e) + { RCLCPP_ERROR(get_logger(), "Error while control mode change: %s", e.what()); return false; } // Activate controllers needed for the new control mode - if (is_active_state) { - if (!switch_controllers.first.empty() && !kuka_drivers_core::changeControllerState( + if (is_active_state) + { + if ( + !switch_controllers.first.empty() && + !kuka_drivers_core::changeControllerState( change_controller_state_client_, switch_controllers.first, {})) { RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); @@ -388,7 +397,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) control_mode_pub_->publish(message); RCLCPP_INFO(get_logger(), "Control mode change process has started"); - if (is_active_state) { + if (is_active_state) + { // The driver is in active state #ifdef NON_MOCK_SETUP @@ -396,14 +406,11 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) std::unique_lock<std::mutex> control_mode_lk(this->control_mode_cv_m_); if (!this->control_mode_cv_.wait_for( - control_mode_lk, std::chrono::milliseconds(2000), [this]() { - return this->control_mode_change_finished_; - })) + control_mode_lk, std::chrono::milliseconds(2000), + [this]() { return this->control_mode_change_finished_; })) { // Control Mode change timeout reached - RCLCPP_ERROR( - get_logger(), - "Timeout reached while waiting for robot to change control mode."); + RCLCPP_ERROR(get_logger(), "Timeout reached while waiting for robot to change control mode."); this->on_deactivate(get_current_state()); return false; } @@ -413,7 +420,9 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) #endif // Deactivate unnecessary controllers - if (!switch_controllers.second.empty() && !kuka_drivers_core::changeControllerState( + if ( + !switch_controllers.second.empty() && + !kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, switch_controllers.second)) { RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); @@ -421,7 +430,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) this->on_deactivate(get_current_state()); return false; } - if (!controller_handler_.ApproveControllerDeactivation()) { + if (!controller_handler_.ApproveControllerDeactivation()) + { RCLCPP_ERROR( get_logger(), "Controller handler state is improper, active controller list was modified" @@ -430,8 +440,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) } RCLCPP_INFO( - get_logger(), "Successfully changed control mode to %s", ExternalControlMode_Name( - control_mode).c_str()); + get_logger(), "Successfully changed control mode to %s", + ExternalControlMode_Name(control_mode).c_str()); param_declared_ = true; return true; } diff --git a/kuka_iiqka_eac_driver/test/test_driver_startup.py b/kuka_iiqka_eac_driver/test/test_driver_startup.py new file mode 100644 index 00000000..0d5b2759 --- /dev/null +++ b/kuka_iiqka_eac_driver/test/test_driver_startup.py @@ -0,0 +1,58 @@ +# Copyright 2024 Áron Svastits +# +# 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 unittest + +import launch +import launch.actions +import launch_testing.actions +import launch_testing.markers +import pytest + +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 +from launch.actions.include_launch_description import IncludeLaunchDescription +from ament_index_python.packages import get_package_share_directory + + +# Launch all of the robot visualisation launch files one by one +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + return launch.LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + get_package_share_directory("kuka_iiqka_eac_driver"), + "/launch/", + "startup.launch.py", + ] + ) + ), + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestModels(unittest.TestCase): + def test_read_stdout(self, proc_output): + # Check for successful initialization + proc_output.assertWaitFor("got segment base", timeout=5) + proc_output.assertWaitFor( + "Successful initialization of hardware 'lbr_iisy3_r760'", timeout=5 + ) + # Check whether disabling automatic activation was successful + proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) diff --git a/kuka_kss_rsi_driver/CMakeLists.txt b/kuka_kss_rsi_driver/CMakeLists.txt index fe7010ee..1d77fa23 100644 --- a/kuka_kss_rsi_driver/CMakeLists.txt +++ b/kuka_kss_rsi_driver/CMakeLists.txt @@ -55,22 +55,8 @@ install(TARGETS ${PROJECT_NAME} robot_manager_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_pep257 REQUIRED) - find_package(ament_cmake_flake8 REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_xmllint REQUIRED) - - ament_cppcheck(--language=c++) - ament_pep257() - ament_flake8() - ament_cpplint() - ament_lint_cmake() - ament_uncrustify(--language=C++) - ament_xmllint(--exclude ros_rsi.rsi.xml) + find_package(launch_testing_ament_cmake) + add_launch_test(test/test_driver_startup.py) endif() ## EXPORTS diff --git a/kuka_kss_rsi_driver/config/driver_config.yaml b/kuka_kss_rsi_driver/config/driver_config.yaml index f037e35e..36466a97 100644 --- a/kuka_kss_rsi_driver/config/driver_config.yaml +++ b/kuka_kss_rsi_driver/config/driver_config.yaml @@ -1,2 +1,2 @@ client_ip: "0.0.0.0" -client_port: 59152 \ No newline at end of file +client_port: 59152 diff --git a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml index 2d4cece3..3f727ef4 100644 --- a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml @@ -1,6 +1,6 @@ joint_trajectory_controller: ros__parameters: - joints: + joints: - joint_1 - joint_2 - joint_3 @@ -15,4 +15,3 @@ joint_trajectory_controller: state_publish_rate: 50.0 action_monitor_rate: 20.0 allow_nonzero_velocity_at_trajectory_end: True - diff --git a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml index 2eabba85..ee137419 100644 --- a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml @@ -7,5 +7,5 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - + configure_components_on_start: [""] diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp index 5649afc1..ee94efbc 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp @@ -1,63 +1,37 @@ -/********************************************************************* -* 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 - * Author: Svastits Aron - */ +// Copyright 2023 Áron Svastits +// +// 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_KSS_RSI_DRIVER__HARDWARE_INTERFACE_HPP_ #define KUKA_KSS_RSI_DRIVER__HARDWARE_INTERFACE_HPP_ -#include <vector> -#include <string> -#include <memory> #include <chrono> #include <cmath> +#include <memory> #include <mutex> +#include <string> +#include <vector> -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/macros.hpp" #include "pluginlib/class_list_macros.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "hardware_interface/system_interface.hpp" -#include "kuka_kss_rsi_driver/udp_server.h" -#include "kuka_kss_rsi_driver/rsi_state.h" -#include "kuka_kss_rsi_driver/rsi_command.h" +#include "kuka_kss_rsi_driver/rsi_command.hpp" +#include "kuka_kss_rsi_driver/rsi_state.hpp" +#include "kuka_kss_rsi_driver/udp_server.hpp" #include "kuka_kss_rsi_driver/visibility_control.h" using hardware_interface::return_type; diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp index ea96a9e0..c35ba604 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp @@ -15,16 +15,16 @@ #ifndef KUKA_KSS_RSI_DRIVER__ROBOT_MANAGER_NODE_HPP_ #define KUKA_KSS_RSI_DRIVER__ROBOT_MANAGER_NODE_HPP_ -#include <string> +#include <map> #include <memory> +#include <string> #include <vector> -#include <map> -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/client.hpp" -#include "std_msgs/msg/bool.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" @@ -60,7 +60,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> is_configured_pub_; std_msgs::msg::Bool is_configured_msg_; }; -} // namespace kuka_rsi - +} // namespace kuka_rsi #endif // KUKA_KSS_RSI_DRIVER__ROBOT_MANAGER_NODE_HPP_ diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h deleted file mode 100644 index e2b2f6bc..00000000 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h +++ /dev/null @@ -1,85 +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 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. - *********************************************************************/ - -/* - * Author: Lars Tingelstad <lars.tingelstad@ntnu.no> - */ - -#ifndef KUKA_KSS_RSI_DRIVER__RSI_COMMAND_H_ -#define KUKA_KSS_RSI_DRIVER__RSI_COMMAND_H_ - -#include <tinyxml.h> -#include <vector> -#include <string> - -namespace kuka_kss_rsi_driver -{ -class RSICommand -{ -public: - RSICommand() = default; - RSICommand(std::vector<double> joint_position_correction, uint64_t ipoc, bool stop = false) - { - TiXmlDocument doc; - TiXmlElement * root = new TiXmlElement("Sen"); - root->SetAttribute("Type", "KROSHU"); - 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])); - el->SetAttribute("A3", std::to_string(joint_position_correction[2])); - el->SetAttribute("A4", std::to_string(joint_position_correction[3])); - el->SetAttribute("A5", std::to_string(joint_position_correction[4])); - el->SetAttribute("A6", std::to_string(joint_position_correction[5])); - root->LinkEndChild(el); - - el = new TiXmlElement("Stop"); - el->LinkEndChild(new TiXmlText(std::to_string(static_cast<int>(stop)))); - root->LinkEndChild(el); - - el = new TiXmlElement("IPOC"); - el->LinkEndChild(new TiXmlText(std::to_string(ipoc))); - root->LinkEndChild(el); - doc.LinkEndChild(root); - TiXmlPrinter printer; - printer.SetStreamPrinting(); - doc.Accept(&printer); - - xml_doc = printer.Str(); - } - std::string xml_doc; -}; -} // namespace kuka_kss_rsi_driver - -#endif // KUKA_KSS_RSI_DRIVER__RSI_COMMAND_H_ diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.hpp new file mode 100644 index 00000000..1d7bafed --- /dev/null +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 Lars Tingelstad +// +// 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_KSS_RSI_DRIVER__RSI_COMMAND_H_ +#define KUKA_KSS_RSI_DRIVER__RSI_COMMAND_H_ + +#include <tinyxml.h> +#include <string> +#include <vector> + +namespace kuka_kss_rsi_driver +{ +class RSICommand +{ +public: + RSICommand() = default; + RSICommand(std::vector<double> joint_position_correction, uint64_t ipoc, bool stop = false) + { + TiXmlDocument doc; + TiXmlElement * root = new TiXmlElement("Sen"); + root->SetAttribute("Type", "KROSHU"); + 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])); + el->SetAttribute("A3", std::to_string(joint_position_correction[2])); + el->SetAttribute("A4", std::to_string(joint_position_correction[3])); + el->SetAttribute("A5", std::to_string(joint_position_correction[4])); + el->SetAttribute("A6", std::to_string(joint_position_correction[5])); + root->LinkEndChild(el); + + el = new TiXmlElement("Stop"); + el->LinkEndChild(new TiXmlText(std::to_string(static_cast<int>(stop)))); + root->LinkEndChild(el); + + el = new TiXmlElement("IPOC"); + el->LinkEndChild(new TiXmlText(std::to_string(ipoc))); + root->LinkEndChild(el); + doc.LinkEndChild(root); + TiXmlPrinter printer; + printer.SetStreamPrinting(); + doc.Accept(&printer); + + xml_doc = printer.Str(); + } + std::string xml_doc; +}; +} // namespace kuka_kss_rsi_driver + +#endif // KUKA_KSS_RSI_DRIVER__RSI_COMMAND_H_ diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.hpp similarity index 56% rename from kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.h rename to kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.hpp index 9ff12665..fdf4a1b3 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_state.hpp @@ -1,40 +1,16 @@ -/********************************************************************* - * 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 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. - *********************************************************************/ - -/* - * Author: Lars Tingelstad <lars.tingelstad@ntnu.no> -*/ +// Copyright 2022 Lars Tingelstad +// +// 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_KSS_RSI_DRIVER__RSI_STATE_H_ #define KUKA_KSS_RSI_DRIVER__RSI_STATE_H_ @@ -51,13 +27,9 @@ class RSIState std::string xml_doc_; public: - RSIState() - { - xml_doc_.resize(1024); - } + RSIState() { xml_doc_.resize(1024); } - explicit RSIState(std::string xml_doc) - : xml_doc_(xml_doc) + explicit RSIState(std::string xml_doc) : xml_doc_(xml_doc) { // Parse message from robot TiXmlDocument bufferdoc; diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.hpp similarity index 52% rename from kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h rename to kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.hpp index fd455d00..76ef216d 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.hpp @@ -1,41 +1,16 @@ -/********************************************************************* - * 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 - */ +// Copyright 2022 Lars Tingelstad +// +// 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_KSS_RSI_DRIVER__UDP_SERVER_H_ #define KUKA_KSS_RSI_DRIVER__UDP_SERVER_H_ @@ -43,19 +18,19 @@ // Select includes #include <sys/time.h> -#include <string.h> +#include <arpa/inet.h> +#include <netdb.h> +#include <netinet/in.h> #include <stdio.h> -#include <unistd.h> #include <stdlib.h> -#include <netdb.h> -#include <sys/types.h> +#include <string.h> #include <sys/socket.h> -#include <netinet/in.h> -#include <arpa/inet.h> +#include <sys/types.h> +#include <unistd.h> #include <iostream> -#include <string> #include <stdexcept> +#include <string> #include "rclcpp/rclcpp.hpp" @@ -63,12 +38,12 @@ class UDPServer { public: UDPServer(std::string host, unsigned short port) - : local_host_(host), local_port_(port), timeout_( - false) + : local_host_(host), local_port_(port), timeout_(false) { RCLCPP_INFO(rclcpp::get_logger("UDPServer"), "%s: %i", local_host_.c_str(), local_port_); sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd_ < 0) { + if (sockfd_ < 0) + { throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); } optval = 1; @@ -77,28 +52,29 @@ class UDPServer 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_); } UDPServer(UDPServer & other) = delete; UDPServer & operator=(const UDPServer & other) = delete; bool set_timeout(int millisecs) { - if (millisecs != 0) { + if (millisecs != 0) + { tv_.tv_sec = millisecs / 1000; tv_.tv_usec = (millisecs % 1000) * 1000; timeout_ = true; return timeout_; - } else { + } + else + { return timeout_; } } @@ -107,9 +83,9 @@ class UDPServer { ssize_t bytes = 0; bytes = sendto( - sockfd_, buffer.c_str(), - buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); - if (bytes < 0) { + sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *)&clientaddr_, clientlen_); + if (bytes < 0) + { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send"); } @@ -120,7 +96,8 @@ class UDPServer { ssize_t bytes = 0; - if (timeout_) { + if (timeout_) + { fd_set read_fds; FD_ZERO(&read_fds); FD_SET(sockfd_, &read_fds); @@ -129,25 +106,32 @@ class UDPServer tv.tv_sec = tv_.tv_sec; tv.tv_usec = tv_.tv_usec; - if (select(sockfd_ + 1, &read_fds, nullptr, nullptr, &tv) < 0) { + if (select(sockfd_ + 1, &read_fds, nullptr, nullptr, &tv) < 0) + { return 0; } - if (FD_ISSET(sockfd_, &read_fds)) { + if (FD_ISSET(sockfd_, &read_fds)) + { memset(buffer_, 0, BUFSIZE); bytes = - recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); - if (bytes < 0) { + recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); + if (bytes < 0) + { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } - } else { + } + else + { return 0; } - - } else { + } + else + { memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); - if (bytes < 0) { + bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); + if (bytes < 0) + { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } diff --git a/kuka_kss_rsi_driver/krl/README_KRC4.md b/kuka_kss_rsi_driver/krl/README_KRC4.md deleted file mode 100644 index c080e8ef..00000000 --- a/kuka_kss_rsi_driver/krl/README_KRC4.md +++ /dev/null @@ -1,74 +0,0 @@ -# Configuring RSI on the controller - -This tutorial was tested with RSI 4.1.3 (on KSS8.6) - -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS2. - -## 1. Controller network configuration - -Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. - -1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). -2. There should already be an interface checked out as the **Windows interface**. For example: - * **IP**: 192.168.250.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 - * **Windows interface checkbox** should be checked. -3. Minimize the SmartHMI (**Start-up > Service > Minimize HMI**). -4. Run **RSI-Network** from the Windows Start menu (**All Programs > RSI-Network**). -5. Check that the **Network - Kuka User Interface** show the Windows interface with the specified IP address. -6. Add a new IP address on another subnet (e.g. 192.168.1.20) for the **RSI interface**. - * Select the entry **New** under **RSI Ethernet** in the tree structure and press **Edit**. - * Enter the IP address and confirm with **OK**. - * Close **RSI-Network** and maximize the SmartHMI. -7. Reboot the controller with a cold restart (**Shutdown > Check *Force cold start* and *Reload files* > Reboot control PC**). -8. After reboot, minimize the SmartHMI (**Start-up > Service > Minimize HMI**). -9. Run **cmd.exe** and ping the PC you want to communicate with on the same subnet (e.g. 192.168.250.xx). - -If your **PC** has an IP address on the same subnet as the **Windows interface** on the controller, the controller should receive answers from the PC: -* If this is the case, add another IP address to the current PC connection (e.g. 192.168.1.xx) on the same subnet as the **RSI** interface. - -## 2. KRL Files - -The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. - -##### ros_rsi_ethernet.xml -1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. -2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. - -Note that the `client_ip` and `client_port` parameters in config/driver_config.yaml must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.src -This should only be edited if the start position specified within the file is not desirable for your application. - -##### Copy files to controller -The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: - -1. Copy the files to a USB-stick. -2. Plug it into the teach pad or controller. -3. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. - -## 3. Testing -At this point you are ready to test the RSI interface. Before the test, make sure that: - -* You have specified the `client_ip` and `client_port` tags in the configuration file (config/driver_config.yaml) to correspond with the KRL files on the controller. -* This IP address is available on the client machine (see Network configuration) - -The next steps describe how to start external control using RSI: - -* Start the driver: ```ros2 launch kuka_kss_rsi_driver startup.launch.py``` - -* In a new terminal: ```ros2 lifecycle set robot_manager configure``` - -* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run - * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay -* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` - * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds -* Start step RSI_MOVECORR() withing the given timeout - * in T1 this can be done with confirming the previously described warning - * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. - -After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders -- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver diff --git a/kuka_kss_rsi_driver/krl/README_KRC5.md b/kuka_kss_rsi_driver/krl/README_KRC5.md deleted file mode 100644 index 7ea03a9d..00000000 --- a/kuka_kss_rsi_driver/krl/README_KRC5.md +++ /dev/null @@ -1,87 +0,0 @@ -# Configuring RSI on the controller - -This tutorial was tested with RSI 5.0.2 (on KSS8.7) - -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the KRC5 controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. - -## 1. Controller network configuration - -Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. - -1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). -2. There should already be an interface checked out as the **Windows interface**. For example: - * **IP**: 192.168.250.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty - * **Windows interface checkbox** should be checked. -3. Add a new interface bz pressign the **Advanced** button and **New interface**. -4. Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** -5. Set the IP address to a different subnet then the **KLI interface**. For example: - * **IP**: 192.168.10.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: leave it empty - * **Windows interface checkbox** should NOT be checked -6. Save the changes and reboot the robot controller using the settings **Cold start** and **Reload files** - -## 2. Set fix IP in your PC -1. Set one IP in the subnet of the KLI interface, it is required to connect the WorkVisual and transfer project. For example: - * **IP**: 192.168.250.10 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty -2. Add another IP in the subnet of the RSI interface, it is required to send commands via the RSI interface. For example: - * In windows **Network and Sharing center**, select the internet adapter settings *e.g. Ethernet Properties. - * Select the IPV4 properties and their the **Advanced** settings. - * In the new window, you can **Add** new IP addresses. - -## 3. KRL Files - -The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. - -##### ros_rsi_ethernet.xml -1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. -2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. - -Note that the `client_ip` and `client_port` parameters in config/driver_config.yaml must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.src -This should only be edited if the start position specified within the file is not desirable for your application. - -##### Copy files to controller -The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: - -Method 1: -1. Copy the files to a USB-stick. -2. Plug it into the teach pad or controller. -3. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. - -Method 2: -1. Use the WorkVisual, connect to the KRC -2. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in the WorkVisual -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in the WorkVisual -6. Deploy the project, and follow the orders - -## 4. Testing -At this point you are ready to test the RSI interface. Before the test, make sure that: - -* You have specified the `client_ip` and `client_port` tags in the configuration file (config/driver_config.yaml) to correspond with the KRL files on the controller. -* This IP address is available on the client machine (see Network configuration) - -The next steps describe how to start external control using RSI: - -* Start the driver: ```ros2 launch kuka_kss_rsi_driver startup.launch.py``` - -* In a new terminal: ```ros2 lifecycle set robot_manager configure``` - -* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run - * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay -* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` - * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds -* Start step RSI_MOVECORR() withing the given timeout - * in T1 this can be done with confirming the previously described warning - * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. - -After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders -- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver diff --git a/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi new file mode 100644 index 00000000..af100d71 --- /dev/null +++ b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi @@ -0,0 +1,126 @@ +<?xml version="1.0" encoding="utf-8"?> +<rSIModel dslVersion="1.0.0.0" name="" xmlns="http://schemas.microsoft.com/dsltools/RSIVisual"> + <rSIObjects> + <rSIElement name="AXISCORR1" objType="AXISCORR" objTypeID="33" maxInputs="0" maxOutputs="0"> + <rSIInPorts> + <rSIInPort name="CorrA1" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out1" /> + </source> + </rSIInPort> + <rSIInPort name="CorrA2" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out2" /> + </source> + </rSIInPort> + <rSIInPort name="CorrA3" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out3" /> + </source> + </rSIInPort> + <rSIInPort name="CorrA4" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out4" /> + </source> + </rSIInPort> + <rSIInPort name="CorrA5" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out5" /> + </source> + </rSIInPort> + <rSIInPort name="CorrA6" mandatory="false"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out6" /> + </source> + </rSIInPort> + </rSIInPorts> + <rSIOutPorts> + <rSIOutPort name="Stat" signalType="Int" /> + <rSIOutPort name="A1" /> + <rSIOutPort name="A2" /> + <rSIOutPort name="A3" /> + <rSIOutPort name="A4" /> + <rSIOutPort name="A5" /> + <rSIOutPort name="A6" /> + </rSIOutPorts> + <rSIParameters> + <rSIParameter name="LowerLimA1" value="-170.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="1" /> + <rSIParameter name="LowerLimA2" value="-135.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="2" /> + <rSIParameter name="LowerLimA3" value="-210.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="3" /> + <rSIParameter name="LowerLimA4" value="-185.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="4" /> + <rSIParameter name="LowerLimA5" value="-210.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="5" /> + <rSIParameter name="LowerLimA6" value="-350.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="6" /> + <rSIParameter name="UpperLimA1" value="170.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="13" /> + <rSIParameter name="UpperLimA2" value="90.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="14" /> + <rSIParameter name="UpperLimA3" value="90.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="15" /> + <rSIParameter name="UpperLimA4" value="90.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="16" /> + <rSIParameter name="UpperLimA5" value="120.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="17" /> + <rSIParameter name="UpperLimA6" value="350.0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="18" /> + </rSIParameters> + </rSIElement> + <rSIElement name="ETHERNET1" objType="ETHERNET" objTypeID="64" maxInputs="64" maxOutputs="64"> + <rSIInPorts> + <rSIInPort name="In1" mandatory="false" /> + </rSIInPorts> + <rSIOutPorts> + <rSIOutPort name="Out1" /> + <rSIOutPort name="Out2" /> + <rSIOutPort name="Out3" /> + <rSIOutPort name="Out4" /> + <rSIOutPort name="Out5" /> + <rSIOutPort name="Out6" /> + <rSIOutPort name="Out7" /> + <rSIOutPort name="Out8" /> + </rSIOutPorts> + <rSIParameters> + <rSIParameter name="ConfigFile" value="ros_rsi_ethernet.xml" paramType="System.FileName" minVal="-2147483648" maxVal="2147483647" isEnum="false" isRuntime="false" index="1" /> + <rSIParameter name="Timeout" value="100" paramType="System.Int32" minVal="0" maxVal="2147483647" isEnum="false" index="1" /> + <rSIParameter name="Flag" value="1" paramType="System.Int32" minVal="-1" maxVal="999" isEnum="false" index="4" /> + <rSIParameter name="Precision" value="4" paramType="System.Int32" minVal="1" maxVal="32" isEnum="false" index="8" /> + </rSIParameters> + </rSIElement> + <rSIElement name="AXISCORRMON1" objType="AXISCORRMON" objTypeID="82" maxInputs="0" maxOutputs="0"> + <rSIOutPorts> + <rSIOutPort name="A1" /> + <rSIOutPort name="A2" /> + <rSIOutPort name="A3" /> + <rSIOutPort name="A4" /> + <rSIOutPort name="A5" /> + <rSIOutPort name="A6" /> + <rSIOutPort name="E1" /> + <rSIOutPort name="E2" /> + <rSIOutPort name="E3" /> + <rSIOutPort name="E4" /> + <rSIOutPort name="E5" /> + <rSIOutPort name="E6" /> + </rSIOutPorts> + <rSIParameters> + <rSIParameter name="MaxA1" value="360.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="1" /> + <rSIParameter name="MaxA2" value="200.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="2" /> + <rSIParameter name="MaxA3" value="360.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="3" /> + <rSIParameter name="MaxA4" value="720.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="4" /> + <rSIParameter name="MaxA5" value="360.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="5" /> + <rSIParameter name="MaxA6" value="360.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="6" /> + <rSIParameter name="MaxE1" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="7" /> + <rSIParameter name="MaxE2" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="8" /> + <rSIParameter name="MaxE3" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="9" /> + <rSIParameter name="MaxE4" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="10" /> + <rSIParameter name="MaxE5" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="11" /> + <rSIParameter name="MaxE6" value="6.0" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="12" /> + </rSIParameters> + </rSIElement> + <rSIElement name="STOP1" objType="STOP" objTypeID="18" maxInputs="0" maxOutputs="0"> + <rSIInPorts> + <rSIInPort name="In1" signalType="Bool"> + <source> + <rSIOutPortMoniker name="//ETHERNET1/Out7" /> + </source> + </rSIInPort> + </rSIInPorts> + <rSIParameters> + <rSIParameter name="Mode" value="ExitMoveCorr" paramType="KUKA.RSIVisual.RSI_StopType" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="1" /> + <rSIParameter name="Channel" value="0" paramType="System.Int32" minVal="0" maxVal="2147483647" isEnum="false" index="2" /> + </rSIParameters> + </rSIElement> + </rSIObjects> +</rSIModel> diff --git a/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.diagram b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.diagram new file mode 100644 index 00000000..a0be6778 --- /dev/null +++ b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.diagram @@ -0,0 +1,220 @@ +<?xml version="1.0" encoding="utf-8"?> +<rSIObjectDiagram dslVersion="1.0.0.0" absoluteBounds="0, 0, 15.875, 8.5" name="rsi_axis_corr"> + <rSIModelMoniker name="/" /> + <nestedChildShapes> + <rSIElementShape Id="b2d9a2e7-ac0e-45db-932a-f7115bae64c0" absoluteBounds="10.625, 2.25, 1.5, 3.1670068359374994" fillColor="BlanchedAlmond"> + <rSIElementMoniker name="//AXISCORR1" /> + <relativeChildShapes> + <rSIInPortShape Id="f2b59bf1-531b-44c5-a47a-f7d194e63c71" absoluteBounds="10.224999994039536, 2.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA1" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIInPortShape Id="a1b92ae8-ac01-43de-8aa1-2723ebe1201d" absoluteBounds="10.224999994039536, 2.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA2" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIInPortShape Id="70df0e57-d834-429c-b2b8-3d07c0daf2aa" absoluteBounds="10.224999994039536, 3.3, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA3" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIInPortShape Id="c587778f-7d8f-4e18-91bc-66cdaa987bba" absoluteBounds="10.224999994039536, 3.7, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA4" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIInPortShape Id="2c8328be-e3db-4d34-9730-f95c190abb01" absoluteBounds="10.224999994039536, 4.1, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA5" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIInPortShape Id="1832fdf8-b656-490b-b82d-986bee4e2860" absoluteBounds="10.224999994039536, 4.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//AXISCORR1/CorrA6" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIOutPortShape Id="9af26875-ad2a-4761-8dce-2efce053fe63" absoluteBounds="12.125, 2.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/Stat" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="4b23aece-9f4f-4a3b-baf5-a19e9c27fd1b" absoluteBounds="12.125, 2.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A1" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="5ce0386f-d481-4221-95c0-5c333d528109" absoluteBounds="12.125, 3.3, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A2" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="2ca5ce5a-4390-4093-99bd-624564a65929" absoluteBounds="12.125, 3.7, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A3" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="e7b7e903-a7e9-455f-bbc7-21885d7d3d7d" absoluteBounds="12.125, 4.1, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A4" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="7a504f41-0188-4351-99d7-48255eaf6a0b" absoluteBounds="12.125, 4.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A5" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="7fd76d74-da37-4899-b72b-efdf0d0f0d54" absoluteBounds="12.125, 4.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORR1/A6" /> + <relativeChildShapes /> + </rSIOutPortShape> + </relativeChildShapes> + <nestedChildShapes> + <elementListCompartment Id="d6fe22f5-1a0b-405a-be57-28e980716a7f" absoluteBounds="10.64, 2.76, 1.4700000000000002, 2.5570068359375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" /> + </nestedChildShapes> + </rSIElementShape> + <rSIElementShape Id="878829fa-2780-4bf2-a646-b9b8c83f8e43" absoluteBounds="7.125, 2.25, 1.5, 3.2750000029802329" fillColor="BlanchedAlmond"> + <rSIElementMoniker name="//ETHERNET1" /> + <relativeChildShapes> + <rSIInPortShape Id="abdd0bd3-4335-4e64-8eff-da73d407a69c" absoluteBounds="6.7249999940395355, 2.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIInPortMoniker name="//ETHERNET1/In1" /> + <relativeChildShapes /> + </rSIInPortShape> + <rSIOutPortShape Id="de403588-cb6b-4632-8374-a31518ccccbd" absoluteBounds="8.625, 2.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out1" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="3efc067d-808d-4af0-9d26-db0bdd796200" absoluteBounds="8.625, 2.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out2" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="4e25c1ff-acf7-44fb-8f03-414b64ad9218" absoluteBounds="8.625, 3.3, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out3" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="4477ff66-a90f-4577-b0c8-2f6c2ff082c1" absoluteBounds="8.625, 3.7, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out4" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="c0662312-6615-4fba-80e8-959cadf3ba0f" absoluteBounds="8.625, 4.1, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out5" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="05025b87-624a-49fd-89fd-00e7a57f8539" absoluteBounds="8.625, 4.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out6" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="af103059-b638-4b0e-9077-cb1616dea134" absoluteBounds="8.625, 4.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out7" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="d1ec7154-c9c5-47fa-9790-2b036afc5f2e" absoluteBounds="8.625, 5.3000000000000007, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//ETHERNET1/Out8" /> + <relativeChildShapes /> + </rSIOutPortShape> + </relativeChildShapes> + <nestedChildShapes> + <elementListCompartment Id="cddc66e2-5076-4e31-af9a-6e35a15d3067" absoluteBounds="7.14, 2.76, 1.4700000000000002, 1.0185953776041665" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" /> + </nestedChildShapes> + </rSIElementShape> + <rSIElementShape Id="9eb39b8d-27e9-4f39-a536-e57577e37d39" absoluteBounds="13.75, 2.25, 1.5, 4.8750000029802329" fillColor="BlanchedAlmond"> + <rSIElementMoniker name="//AXISCORRMON1" /> + <relativeChildShapes> + <rSIOutPortShape Id="26943528-4375-47f1-951a-20726d0fee28" absoluteBounds="15.25, 2.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A1" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="76f5b08f-533e-40a9-95c5-f22f78999980" absoluteBounds="15.25, 2.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A2" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="a53f6c8a-2b32-4ae9-875c-24490daca2ed" absoluteBounds="15.25, 3.3, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A3" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="96cfed01-2117-4472-97db-0fe25e892a5e" absoluteBounds="15.25, 3.7, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A4" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="642ef027-b273-4ae0-ad39-0fdc57ff953c" absoluteBounds="15.25, 4.1, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A5" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="f6eb961b-6c07-44b5-bc5e-3f09d1fbe5f0" absoluteBounds="15.25, 4.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/A6" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="31e5276b-795f-4c44-93a3-161ea68c5112" absoluteBounds="15.25, 4.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E1" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="3b81f171-6b0a-4ede-85d5-8aed8cfaf9a3" absoluteBounds="15.25, 5.3000000000000007, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E2" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="497099ea-5c7a-49e9-994d-8e4c28da7776" absoluteBounds="15.25, 5.7, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E3" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="9258426c-a097-4619-9823-5035da85ccd5" absoluteBounds="15.25, 6.1, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E4" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="f483c551-8a7a-42b4-ae74-b8af21724205" absoluteBounds="15.25, 6.5, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E5" /> + <relativeChildShapes /> + </rSIOutPortShape> + <rSIOutPortShape Id="26be6624-3307-49c5-b010-1141945ecb04" absoluteBounds="15.25, 6.9, 0.40000000596046448, 0.075000002980232239" fillColor="White"> + <rSIOutPortMoniker name="//AXISCORRMON1/E6" /> + <relativeChildShapes /> + </rSIOutPortShape> + </relativeChildShapes> + <nestedChildShapes> + <elementListCompartment Id="d77bc4fb-edd3-41cc-a262-edac1a57f425" absoluteBounds="13.765, 2.76, 1.4700000000000002, 2.5570068359375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" /> + </nestedChildShapes> + </rSIElementShape> + <rSISignalConnector edgePoints="[(10.2249999940395 : 2.53750000149012); (9.02500000596046 : 2.53750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="f2b59bf1-531b-44c5-a47a-f7d194e63c71" /> + <rSIOutPortShapeMoniker Id="de403588-cb6b-4632-8374-a31518ccccbd" /> + </nodes> + </rSISignalConnector> + <rSISignalConnector edgePoints="[(10.2249999940395 : 2.93750000149012); (9.02500000596046 : 2.93750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="a1b92ae8-ac01-43de-8aa1-2723ebe1201d" /> + <rSIOutPortShapeMoniker Id="3efc067d-808d-4af0-9d26-db0bdd796200" /> + </nodes> + </rSISignalConnector> + <rSISignalConnector edgePoints="[(10.2249999940395 : 3.33750000149012); (9.02500000596046 : 3.33750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="70df0e57-d834-429c-b2b8-3d07c0daf2aa" /> + <rSIOutPortShapeMoniker Id="4e25c1ff-acf7-44fb-8f03-414b64ad9218" /> + </nodes> + </rSISignalConnector> + <rSISignalConnector edgePoints="[(10.2249999940395 : 3.73750000149012); (9.02500000596046 : 3.73750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="c587778f-7d8f-4e18-91bc-66cdaa987bba" /> + <rSIOutPortShapeMoniker Id="4477ff66-a90f-4577-b0c8-2f6c2ff082c1" /> + </nodes> + </rSISignalConnector> + <rSISignalConnector edgePoints="[(10.2249999940395 : 4.13750000149012); (9.02500000596046 : 4.13750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="2c8328be-e3db-4d34-9730-f95c190abb01" /> + <rSIOutPortShapeMoniker Id="c0662312-6615-4fba-80e8-959cadf3ba0f" /> + </nodes> + </rSISignalConnector> + <rSISignalConnector edgePoints="[(10.2249999940395 : 4.53750000149012); (9.02500000596046 : 4.53750000149012)]" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="1832fdf8-b656-490b-b82d-986bee4e2860" /> + <rSIOutPortShapeMoniker Id="05025b87-624a-49fd-89fd-00e7a57f8539" /> + </nodes> + </rSISignalConnector> + <rSIElementShape Id="0ea9b164-3055-4628-a4f7-92003fa2989a" absoluteBounds="10.5, 6.5, 1.75, 1.2439925130208325" fillColor="BlanchedAlmond"> + <rSIElementMoniker name="//STOP1" /> + <relativeChildShapes> + <rSIInPortShape Id="122f88b3-32fa-41e0-891f-52121a1e8592" absoluteBounds="10.099999994039536, 6.75, 0.40000000596046448, 0.075000002980232239" fillColor="BlanchedAlmond"> + <rSIInPortMoniker name="//STOP1/In1" /> + <relativeChildShapes /> + </rSIInPortShape> + </relativeChildShapes> + <nestedChildShapes> + <elementListCompartment Id="4349d8ef-d1a3-46aa-a2a1-01e19ac1a04a" absoluteBounds="10.515, 7.01, 1.7200000000000002, 0.63399251302083326" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" /> + </nestedChildShapes> + </rSIElementShape> + <rSISignalConnector edgePoints="[(10.0999999940395 : 6.78750000149012); (9.70104166368644 : 6.78750000149012); (9.70104166368644 : 4.93750000149012); (9.02500000596046 : 4.93750000149012)]" manuallyRouted="true" fixedFrom="Caller" fixedTo="Caller" TargetRelationshipDomainClassId="e3f2b7da-330d-4daf-bd29-4d4ed734c0af"> + <nodes> + <rSIInPortShapeMoniker Id="122f88b3-32fa-41e0-891f-52121a1e8592" /> + <rSIOutPortShapeMoniker Id="af103059-b638-4b0e-9077-cb1616dea134" /> + </nodes> + </rSISignalConnector> + </nestedChildShapes> +</rSIObjectDiagram> diff --git a/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.xml b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.xml new file mode 100644 index 00000000..ad11923a --- /dev/null +++ b/kuka_kss_rsi_driver/krl/deprecated/ros_rsi.rsi.xml @@ -0,0 +1,60 @@ +<?xml version="1.0" encoding="UTF-8" standalone="yes"?> +<RSIObjects xsi:noNamespaceSchemaLocation="/Roboter/Config/System/Common/Schemes/RSIContext.xsd" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"> + <RSIObject ObjType="AXISCORR" ObjTypeID="33" ObjID="AXISCORR1"> + <Inputs> + <Input InIdx="1" OutObjID="ETHERNET1" OutIdx="1" /> + <Input InIdx="2" OutObjID="ETHERNET1" OutIdx="2" /> + <Input InIdx="3" OutObjID="ETHERNET1" OutIdx="3" /> + <Input InIdx="4" OutObjID="ETHERNET1" OutIdx="4" /> + <Input InIdx="5" OutObjID="ETHERNET1" OutIdx="5" /> + <Input InIdx="6" OutObjID="ETHERNET1" OutIdx="6" /> + </Inputs> + <Parameters> + <Parameter Name="LowerLimA1" ParamID="1" ParamValue="-170.0" /> + <Parameter Name="LowerLimA2" ParamID="2" ParamValue="-135.0" /> + <Parameter Name="LowerLimA3" ParamID="3" ParamValue="-210.0" /> + <Parameter Name="LowerLimA4" ParamID="4" ParamValue="-185.0" /> + <Parameter Name="LowerLimA5" ParamID="5" ParamValue="-210.0" /> + <Parameter Name="LowerLimA6" ParamID="6" ParamValue="-350.0" /> + <Parameter Name="UpperLimA1" ParamID="13" ParamValue="170.0" /> + <Parameter Name="UpperLimA2" ParamID="14" ParamValue="90.0" /> + <Parameter Name="UpperLimA3" ParamID="15" ParamValue="90.0" /> + <Parameter Name="UpperLimA4" ParamID="16" ParamValue="90.0" /> + <Parameter Name="UpperLimA5" ParamID="17" ParamValue="120.0" /> + <Parameter Name="UpperLimA6" ParamID="18" ParamValue="350.0" /> + </Parameters> + </RSIObject> + <RSIObject ObjType="ETHERNET" ObjTypeID="64" ObjID="ETHERNET1"> + <Parameters> + <Parameter Name="ConfigFile" ParamID="1" ParamValue="ros_rsi_ethernet.xml" IsRuntime="false" /> + <Parameter Name="Timeout" ParamID="1" ParamValue="100" /> + <Parameter Name="Flag" ParamID="4" ParamValue="1" /> + <Parameter Name="Precision" ParamID="8" ParamValue="4" /> + </Parameters> + </RSIObject> + <RSIObject ObjType="AXISCORRMON" ObjTypeID="82" ObjID="AXISCORRMON1"> + <Parameters> + <Parameter Name="MaxA1" ParamID="1" ParamValue="360.0" /> + <Parameter Name="MaxA2" ParamID="2" ParamValue="200.0" /> + <Parameter Name="MaxA3" ParamID="3" ParamValue="360.0" /> + <Parameter Name="MaxA4" ParamID="4" ParamValue="720.0" /> + <Parameter Name="MaxA5" ParamID="5" ParamValue="360.0" /> + <Parameter Name="MaxA6" ParamID="6" ParamValue="360.0" /> + <Parameter Name="MaxE1" ParamID="7" ParamValue="6.0" /> + <Parameter Name="MaxE2" ParamID="8" ParamValue="6.0" /> + <Parameter Name="MaxE3" ParamID="9" ParamValue="6.0" /> + <Parameter Name="MaxE4" ParamID="10" ParamValue="6.0" /> + <Parameter Name="MaxE5" ParamID="11" ParamValue="6.0" /> + <Parameter Name="MaxE6" ParamID="12" ParamValue="6.0" /> + </Parameters> + </RSIObject> + <RSIObject ObjType="STOP" ObjTypeID="18" ObjID="STOP1"> + <Inputs> + <Input InIdx="1" OutObjID="ETHERNET1" OutIdx="7" /> + </Inputs> + <Parameters> + <Parameter Name="Mode" ParamID="1" ParamValue="4" /> + <Parameter Name="Channel" ParamID="2" ParamValue="0" /> + </Parameters> + </RSIObject> +</RSIObjects> diff --git a/kuka_kss_rsi_driver/krl/ros_rsi.rsix b/kuka_kss_rsi_driver/krl/ros_rsi.rsix index 4e95c079..5cd6aa63 100755 --- a/kuka_kss_rsi_driver/krl/ros_rsi.rsix +++ b/kuka_kss_rsi_driver/krl/ros_rsi.rsix @@ -1,4 +1,4 @@ -<?xml version="1.0" encoding="utf-8"?> +<?xml version="1.0" encoding="utf-8"?> <!-- Do not change! The content of this file is generated. --> <!-- Generated with RSIVisual WorkVisual plugin. --> <!-- RobotSensorInterface 5.0 B356 --> @@ -2434,4 +2434,4 @@ </Inputs> </Construct> </Constructs> -</RsiContext> \ No newline at end of file +</RsiContext> diff --git a/kuka_kss_rsi_driver/krl/ros_rsi.src b/kuka_kss_rsi_driver/krl/ros_rsi.src index 28ec7f79..3b7eaf29 100644 --- a/kuka_kss_rsi_driver/krl/ros_rsi.src +++ b/kuka_kss_rsi_driver/krl/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_kss_rsi_driver ; ; ============================================= @@ -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 @@ -65,7 +65,7 @@ DECL INT CONTID ; ContainerID ;ENDFOLD (USER INI) ;ENDFOLD (INI) -; Create RSI Context +; Create RSI Context ret = RSI_CREATE("ros_rsi",CONTID,TRUE) IF (ret <> RSIOK) THEN HALT @@ -80,7 +80,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_kss_rsi_driver/krl/ros_rsi_ethernet.xml b/kuka_kss_rsi_driver/krl/ros_rsi_ethernet.xml index aadb5a8a..0558213e 100755 --- a/kuka_kss_rsi_driver/krl/ros_rsi_ethernet.xml +++ b/kuka_kss_rsi_driver/krl/ros_rsi_ethernet.xml @@ -1,17 +1,17 @@ <ROOT> <CONFIG> <IP_NUMBER>ip.address.of.rospc</IP_NUMBER> <!-- IP-number of the external socket --> - <PORT>port.of.rospc</PORT> <!-- Port-number of the external socket --> - <SENTYPE>KROSHU</SENTYPE> <!-- The name of your system send in <Sen Type="" > --> + <PORT>59152</PORT> <!-- Port-number of the external socket --> + <SENTYPE>KROSHU</SENTYPE> <!-- The name of your system send in <Sen Type="" > --> <ONLYSEND>FALSE</ONLYSEND> <!-- TRUE means the client don't expect answers. Do not send anything to robot --> </CONFIG> <!-- RSI Data: TYPE= "BOOL", "STRING", "LONG", "DOUBLE" --> <!-- INDX= "INTERNAL" switch on internal read values. Needed by DEF_... --> - <!-- INDX= "nmb" Input/Output index of RSI-Object / Maximum of RSI Channels: 64 --> - <!-- HOLDON="1", set this output index of RSI Object to the last value --> + <!-- INDX= "nmb" Input/Output index of RSI-Object / Maximum of RSI Channels: 64 --> + <!-- HOLDON="1", set this output index of RSI Object to the last value --> <!-- DEF_Delay count the late packages and send it back to server --> <!-- DEF_Tech: .C = advance .T = main run / .C1 advance set function generator 1 --> - + <SEND> <ELEMENTS> <ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" /> @@ -32,4 +32,4 @@ <ELEMENT TAG="Stop" TYPE="BOOL" INDX="7" HOLDON="0" /> </ELEMENTS> </RECEIVE> -</ROOT> \ No newline at end of file +</ROOT> diff --git a/kuka_kss_rsi_driver/launch/startup.launch.py b/kuka_kss_rsi_driver/launch/startup.launch.py index 25455a79..fad88fef 100644 --- a/kuka_kss_rsi_driver/launch/startup.launch.py +++ b/kuka_kss_rsi_driver/launch/startup.launch.py @@ -23,9 +23,9 @@ def launch_setup(context, *args, **kwargs): - robot_model = LaunchConfiguration('robot_model') - robot_family = LaunchConfiguration('robot_family') - use_fake_hardware = LaunchConfiguration('use_fake_hardware') + robot_model = LaunchConfiguration("robot_model") + robot_family = LaunchConfiguration("robot_family") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") # Get URDF via xacro robot_description_content = Command( @@ -33,68 +33,77 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare('kuka_{}_support'.format(robot_family.perform(context))), - "urdf", robot_model.perform(context) + ".urdf.xacro"] + [ + FindPackageShare(f"kuka_{robot_family.perform(context)}_support"), + "urdf", + robot_model.perform(context) + ".urdf.xacro", + ] ), " ", "use_fake_hardware:=", use_fake_hardware, - ], on_stderr='capture' + ], + on_stderr="capture", ) - robot_description = {'robot_description': robot_description_content} + robot_description = {"robot_description": robot_description_content} - controller_config = (get_package_share_directory('kuka_kss_rsi_driver') + - "/config/ros2_controller_config.yaml") + controller_config = ( + get_package_share_directory("kuka_kss_rsi_driver") + "/config/ros2_controller_config.yaml" + ) - joint_traj_controller_config = (get_package_share_directory('kuka_kss_rsi_driver') + - "/config/joint_trajectory_controller_config.yaml") + joint_traj_controller_config = ( + get_package_share_directory("kuka_kss_rsi_driver") + + "/config/joint_trajectory_controller_config.yaml" + ) - controller_manager_node = '/controller_manager' + controller_manager_node = "/controller_manager" control_node = Node( - package='kuka_drivers_core', - executable='control_node', - parameters=[robot_description, controller_config] + package="kuka_drivers_core", + executable="control_node", + parameters=[robot_description, controller_config], ) robot_manager_node = LifecycleNode( - name=['robot_manager'], - namespace='', + name=["robot_manager"], + namespace="", package="kuka_kss_rsi_driver", executable="robot_manager_node", - parameters=[{'robot_model': robot_model}] + parameters=[{"robot_model": robot_model}], ) robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], ) # Spawn controllers def controller_spawner(controller_with_config, activate=False): - arg_list = [controller_with_config[0], "-c", controller_manager_node, "-p", - controller_with_config[1]] + arg_list = [ + controller_with_config[0], + "-c", + controller_manager_node, + "-p", + controller_with_config[1], + ] if not activate: arg_list.append("--inactive") - return Node( - package="controller_manager", - executable="spawner", - arguments=arg_list - ) + return Node(package="controller_manager", executable="spawner", arguments=arg_list) controller_names_and_config = [ ("joint_state_broadcaster", []), ("joint_trajectory_controller", joint_traj_controller_config), ] - controller_spawners = [controller_spawner(controllers) - for controllers in controller_names_and_config] + controller_spawners = [ + controller_spawner(controllers) for controllers in controller_names_and_config + ] nodes_to_start = [ control_node, robot_manager_node, - robot_state_publisher + robot_state_publisher, ] + controller_spawners return nodes_to_start @@ -102,16 +111,7 @@ def controller_spawner(controller_with_config, activate=False): def generate_launch_description(): launch_arguments = [] - launch_arguments.append(DeclareLaunchArgument( - 'robot_model', - default_value='kr6_r700_sixx' - )) - launch_arguments.append(DeclareLaunchArgument( - 'robot_family', - default_value='agilus' - )) - launch_arguments.append(DeclareLaunchArgument( - 'use_fake_hardware', - default_value="false" - )) + launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="kr6_r700_sixx")) + launch_arguments.append(DeclareLaunchArgument("robot_family", default_value="agilus")) + launch_arguments.append(DeclareLaunchArgument("use_fake_hardware", default_value="false")) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py b/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py index ec5f50ab..f61b321f 100644 --- a/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py +++ b/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py @@ -19,24 +19,31 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch.actions.include_launch_description import IncludeLaunchDescription -from launch.launch_description_sources.python_launch_description_source import PythonLaunchDescriptionSource # noqa: E501 +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 def generate_launch_description(): rviz_config_file = os.path.join( - get_package_share_directory('kuka_resources'), 'config', 'view_6_axis_urdf.rviz') + get_package_share_directory("kuka_resources"), "config", "view_6_axis_urdf.rviz" + ) - startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( - [get_package_share_directory('kuka_kss_rsi_driver'), '/launch/startup.launch.py'])) - - return LaunchDescription([ - startup_launch, - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file, - "--ros-args", "--log-level", "error"], + startup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory("kuka_kss_rsi_driver"), "/launch/startup.launch.py"] ) - ]) + ) + + return LaunchDescription( + [ + startup_launch, + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, "--ros-args", "--log-level", "error"], + ), + ] + ) diff --git a/kuka_kss_rsi_driver/package.xml b/kuka_kss_rsi_driver/package.xml index 83b5a5cd..6879c837 100644 --- a/kuka_kss_rsi_driver/package.xml +++ b/kuka_kss_rsi_driver/package.xml @@ -24,14 +24,6 @@ <exec_depend>ros2_control</exec_depend> <exec_depend>ros2_controllers</exec_depend> - <test_depend>ament_cmake_copyright</test_depend> - <test_depend>ament_cmake_cppcheck</test_depend> - <test_depend>ament_cmake_pep257</test_depend> - <test_depend>ament_cmake_flake8</test_depend> - <test_depend>ament_cmake_cpplint</test_depend> - <test_depend>ament_cmake_lint_cmake</test_depend> - <test_depend>ament_cmake_xmllint</test_depend> - <export> <build_type>ament_cmake</build_type> </export> diff --git a/kuka_kss_rsi_driver/src/hardware_interface.cpp b/kuka_kss_rsi_driver/src/hardware_interface.cpp index ab2a21c1..5c3dc1e9 100644 --- a/kuka_kss_rsi_driver/src/hardware_interface.cpp +++ b/kuka_kss_rsi_driver/src/hardware_interface.cpp @@ -1,46 +1,20 @@ -/********************************************************************* -* 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 - * Author: Svastits Aron - */ +// Copyright 2023 Áron Svastits +// +// 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 <memory> #include <stdexcept> #include <string> -#include <memory> #include <vector> #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -51,45 +25,48 @@ namespace kuka_kss_rsi_driver { CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } hw_states_.resize(info_.joints.size(), 0.0); hw_commands_.resize(info_.joints.size(), 0.0); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 1) { + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != 1) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); + rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "expecting only POSITION command interface"); + rclcpp::get_logger("KukaRSIHardwareInterface"), + "expecting only POSITION command interface"); return CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 1) { + if (joint.state_interfaces.size() != 1) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); + rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); return CallbackReturn::ERROR; } - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "expecting only POSITION state interface"); + rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting only POSITION state interface"); return CallbackReturn::ERROR; } } // RSI - in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 + in_buffer_.resize(1024); out_buffer_.resize(1024); initial_joint_pos_.resize(info_.joints.size(), 0.0); @@ -100,8 +77,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw rsi_port_ = std::stoi(info_.hardware_parameters["client_port"]); RCLCPP_INFO( - rclcpp::get_logger("KukaRSIHardwareInterface"), - "IP of client machine: %s:%d", rsi_ip_address_.c_str(), rsi_port_); + rclcpp::get_logger("KukaRSIHardwareInterface"), "IP of client machine: %s:%d", + rsi_ip_address_.c_str(), rsi_port_); return CallbackReturn::SUCCESS; } @@ -109,24 +86,22 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw std::vector<hardware_interface::StateInterface> KukaRSIHardwareInterface::export_state_interfaces() { std::vector<hardware_interface::StateInterface> state_interfaces; - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { state_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_states_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]); } return state_interfaces; } -std::vector<hardware_interface::CommandInterface> KukaRSIHardwareInterface:: -export_command_interfaces() +std::vector<hardware_interface::CommandInterface> +KukaRSIHardwareInterface::export_command_interfaces() { std::vector<hardware_interface::CommandInterface> command_interfaces; - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { command_interfaces.emplace_back( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]); } return command_interfaces; } @@ -138,11 +113,11 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); server_->set_timeout(10000); // Set receive timeout to 10 seconds for activation - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connecting to robot . . ."); int bytes = server_->recv(in_buffer_); - if (bytes == 0) { + if (bytes == 0) + { RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connection timeout"); return CallbackReturn::FAILURE; } @@ -150,13 +125,15 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got data from robot"); // Drop empty <rob> frame with RSI <= 2.3 - if (bytes < 100) { + if (bytes < 100) + { bytes = server_->recv(in_buffer_); } rsi_state_ = RSIState(in_buffer_); - for (size_t i = 0; i < info_.joints.size(); ++i) { + for (size_t i = 0; i < info_.joints.size(); ++i) + { hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; @@ -173,63 +150,64 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta return CallbackReturn::SUCCESS; } -CallbackReturn KukaRSIHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State &) +CallbackReturn KukaRSIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { stop_flag_ = true; RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Stop flag was set!"); return CallbackReturn::SUCCESS; } -return_type KukaRSIHardwareInterface::read( - const rclcpp::Time &, - const rclcpp::Duration &) +return_type KukaRSIHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { - if (!is_active_) { + if (!is_active_) + { std::this_thread::sleep_for(std::chrono::milliseconds(2)); return return_type::OK; } - if (server_->recv(in_buffer_) == 0) { + if (server_->recv(in_buffer_) == 0) + { RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot"); this->on_deactivate(this->get_state()); return return_type::ERROR; } rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < info_.joints.size(); ++i) { + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; return return_type::OK; } -return_type KukaRSIHardwareInterface::write( - const rclcpp::Time &, - const rclcpp::Duration &) +return_type KukaRSIHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) { // It is possible, that write is called immediately after activation // In this case write in that tick should be skipped to be able to read state at first // First cycle (with 0 ipoc) is handled in the on_activate method, so 0 ipoc means // read was not called yet - if (!is_active_ || ipoc_ == 0) { + if (!is_active_ || ipoc_ == 0) + { return return_type::OK; } - if (stop_flag_) {is_active_ = false;} + if (stop_flag_) + { + is_active_ = false; + } - for (size_t i = 0; i < info_.joints.size(); i++) { - joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * - KukaRSIHardwareInterface::R2D; + for (size_t i = 0; i < info_.joints.size(); i++) + { + joint_pos_correction_deg_[i] = + (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; } out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); return return_type::OK; } -} // namespace namespace kuka_kss_rsi_driver +} // namespace kuka_kss_rsi_driver PLUGINLIB_EXPORT_CLASS( - kuka_kss_rsi_driver::KukaRSIHardwareInterface, - hardware_interface::SystemInterface -) + kuka_kss_rsi_driver::KukaRSIHardwareInterface, hardware_interface::SystemInterface) diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index 3ab0fb9b..c2f2feb0 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -18,38 +18,31 @@ #include "kuka_kss_rsi_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT -using namespace lifecycle_msgs::msg; // NOLINT +using namespace lifecycle_msgs::msg; // NOLINT namespace kuka_rsi { -RobotManagerNode::RobotManagerNode() -: kuka_drivers_core::ROS2BaseLCNode("robot_manager") +RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager") { auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - change_hardware_state_client_ = - this->create_client<SetHardwareComponentState>( - "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_ - ); - change_controller_state_client_ = - this->create_client<SwitchController>( - "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_ - ); + change_hardware_state_client_ = this->create_client<SetHardwareComponentState>( + "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); + change_controller_state_client_ = this->create_client<SwitchController>( + "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_); auto is_configured_qos = rclcpp::QoS(rclcpp::KeepLast(1)); is_configured_qos.best_effort(); - is_configured_pub_ = this->create_publisher<std_msgs::msg::Bool>( - "robot_manager/is_configured", - is_configured_qos); + is_configured_pub_ = + this->create_publisher<std_msgs::msg::Bool>("robot_manager/is_configured", is_configured_qos); this->registerStaticParameter<std::string>( "robot_model", "kr6_r700_sixx", - kuka_drivers_core::ParameterSetAccessRights{true, false, - false, false, false}, [this](const std::string & robot_model) { - return this->onRobotModelChangeRequest(robot_model); - }); + kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + [this](const std::string & robot_model) + { return this->onRobotModelChangeRequest(robot_model); }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -57,8 +50,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_INACTIVE)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; @@ -75,14 +67,14 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Clean up hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_UNCONFIGURED)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_UNCONFIGURED)) { RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } - if (is_configured_pub_->is_activated()) { + if (is_configured_pub_->is_activated()) + { is_configured_msg_.data = false; is_configured_pub_->publish(is_configured_msg_); is_configured_pub_->on_deactivate(); @@ -97,8 +89,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_ACTIVE, 10000)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_ACTIVE, 10000)) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); return FAILURE; @@ -106,8 +97,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate RT controller(s) if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"}, - {})) + change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"}, + {})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); // TODO(Svastits): this can be removed if rollback is implemented properly @@ -124,8 +115,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) { // Deactivate hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - State::PRIMARY_STATE_INACTIVE)) + change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; @@ -134,9 +124,9 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - {"joint_state_broadcaster", "joint_trajectory_controller"}, - SwitchController::Request::BEST_EFFORT)) + change_controller_state_client_, {}, + {"joint_state_broadcaster", "joint_trajectory_controller"}, + SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); return ERROR; diff --git a/kuka_kss_rsi_driver/test/test_driver_startup.py b/kuka_kss_rsi_driver/test/test_driver_startup.py new file mode 100644 index 00000000..95aece53 --- /dev/null +++ b/kuka_kss_rsi_driver/test/test_driver_startup.py @@ -0,0 +1,58 @@ +# Copyright 2024 Áron Svastits +# +# 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 unittest + +import launch +import launch.actions +import launch_testing.actions +import launch_testing.markers +import pytest + +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 +from launch.actions.include_launch_description import IncludeLaunchDescription +from ament_index_python.packages import get_package_share_directory + + +# Launch all of the robot visualisation launch files one by one +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + return launch.LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + get_package_share_directory("kuka_kss_rsi_driver"), + "/launch/", + "startup.launch.py", + ] + ) + ), + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestModels(unittest.TestCase): + def test_read_stdout(self, proc_output): + # Check for successful initialization + proc_output.assertWaitFor("got segment base", timeout=5) + proc_output.assertWaitFor( + "Successful initialization of hardware 'kr6_r700_sixx'", timeout=5 + ) + # Check whether disabling automatic activation was successful + proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) 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 00000000..e69de29b diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py index 5335d2aa..3a67680f 100755 --- a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py +++ b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py @@ -17,8 +17,6 @@ import xml.etree.ElementTree as ET import numpy as np - -import errno import rclpy from rclpy.node import Node from std_msgs.msg import String @@ -27,27 +25,50 @@ 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) + 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 - stop_flag = root.find('Stop').text + 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 + stop_flag = root.find("Stop").text return desired_joint_correction, int(IPOC), bool(int(stop_flag)) @@ -59,54 +80,58 @@ class RSISimulator(Node): des_joint_correction_absolute = np.zeros(6) timeout_count = 0 ipoc = 0 - rsi_ip_address_ = '127.0.0.1' + rsi_ip_address_ = "127.0.0.1" rsi_port_address_ = 59152 - rsi_send_name_ = 'IamFree' + rsi_send_name_ = "IamFree" rsi_act_pub_ = None rsi_cmd_pub_ = None - node_name_ = 'rsi_simulator_node' + node_name_ = "rsi_simulator_node" socket_ = None def __init__(self, node_name): super().__init__(node_name) self.node_name_ = node_name self.timer = self.create_timer(self.cycle_time, self.timer_callback) - self.declare_parameter('rsi_ip_address', '127.0.0.1') - self.declare_parameter('rsi_port', 59152) - self.declare_parameter('rsi_send_name', "IamFree") - self.rsi_ip_address_ = (self.get_parameter('rsi_ip_address').get_parameter_value() - .string_value) - self.rsi_port_address_ = self.get_parameter('rsi_port').get_parameter_value().integer_value - self.rsi_send_name_ = (self.get_parameter('rsi_send_name').get_parameter_value() - .string_value) - self.rsi_act_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/state', 1) - self.rsi_cmd_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/command', 1) - self.get_logger().info('rsi_ip_address: {}'.format(self.rsi_ip_address_)) - self.get_logger().info('rsi_port: {}'.format(self.rsi_port_address_)) + self.declare_parameter("rsi_ip_address", "127.0.0.1") + self.declare_parameter("rsi_port", 59152) + self.declare_parameter("rsi_send_name", "IamFree") + self.rsi_ip_address_ = ( + self.get_parameter("rsi_ip_address").get_parameter_value().string_value + ) + self.rsi_port_address_ = self.get_parameter("rsi_port").get_parameter_value().integer_value + self.rsi_send_name_ = ( + self.get_parameter("rsi_send_name").get_parameter_value().string_value + ) + self.rsi_act_pub_ = self.create_publisher(String, self.node_name_ + "/rsi/state", 1) + self.rsi_cmd_pub_ = self.create_publisher(String, self.node_name_ + "/rsi/command", 1) + self.get_logger().info(f"rsi_ip_address: {self.rsi_ip_address_}") + self.get_logger().info(f"rsi_port: {self.rsi_port_address_}") self.socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.get_logger().info('{}, Successfully created socket'.format(self.node_name_)) + self.get_logger().info(f"{self.node_name_}, Successfully created socket") self.socket_.settimeout(self.cycle_time) def timer_callback(self): if self.timeout_count == 100: - self.get_logger().fatal('{} Timeout count of 100 exceeded'.format(self.node_name_)) + self.get_logger().fatal(f"{self.node_name_} Timeout count of 100 exceeded") sys.exit() try: - msg = create_rsi_xml_rob(self.act_joint_pos, self.initial_joint_pos, - self.timeout_count, self.ipoc) + msg = create_rsi_xml_rob( + self.act_joint_pos, self.initial_joint_pos, self.timeout_count, self.ipoc + ) self.rsi_act_pub_.publish(msg) self.socket_.sendto(msg, (self.rsi_ip_address_, self.rsi_port_address_)) recv_msg, addr = self.socket_.recvfrom(1024) self.rsi_cmd_pub_.publish(recv_msg) - self.get_logger().warn('msg: {}'.format(recv_msg)) + self.get_logger().warn(f"msg: {recv_msg}") des_joint_correction_absolute, ipoc_recv, stop_flag = parse_rsi_xml_sen(recv_msg) if ipoc_recv == self.ipoc: self.act_joint_pos = self.initial_joint_pos + des_joint_correction_absolute else: - self.get_logger().warn('{}: Packet is late'.format(self.node_name_)) - self.get_logger().warn('{}: sent ipoc: {}, received: {}'. - format(self.node_name_, self.ipoc, ipoc_recv)) + self.get_logger().warn(f"{self.node_name_}: Packet is late") + self.get_logger().warn( + f"{self.node_name_}: sent ipoc: {self.ipoc}, received: {ipoc_recv}" + ) if self.ipoc != 0: self.timeout_count += 1 self.ipoc += 1 @@ -116,24 +141,24 @@ def timer_callback(self): except OSError: if self.ipoc != 0: self.timeout_count += 1 - self.get_logger().warn('{}: Socket timed out'.format(self.node_name_)) + self.get_logger().warn(f"{self.node_name_}: Socket timed out") def on_shutdown(self): self.socket_.close() - self.get_logger().info('Socket closed.') + self.get_logger().info("Socket closed.") def main(): - node_name = 'rsi_simulator_node' + node_name = "rsi_simulator_node" rclpy.init() node = RSISimulator(node_name) - node.get_logger().info('{}: Started'.format(node_name)) + node.get_logger().info(f"{node_name}: Started") rclpy.spin(node) node.on_shutdown() - node.get_logger().info('{}: Shutting down'.format(node_name)) + node.get_logger().info(f"{node_name}: Shutting down") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py b/kuka_rsi_simulator/launch/kuka_rsi_simulator.launch.py similarity index 57% rename from kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py rename to kuka_rsi_simulator/launch/kuka_rsi_simulator.launch.py index a683f4ce..048dfc27 100644 --- a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py +++ b/kuka_rsi_simulator/launch/kuka_rsi_simulator.launch.py @@ -22,22 +22,24 @@ def generate_launch_description(): rsi_ip_address = DeclareLaunchArgument( - 'rsi_ip_address', default_value=TextSubstitution(text='127.0.0.1') - ) - rsi_port = DeclareLaunchArgument( - 'rsi_port', default_value=TextSubstitution(text='59152') + "rsi_ip_address", default_value=TextSubstitution(text="127.0.0.1") ) + rsi_port = DeclareLaunchArgument("rsi_port", default_value=TextSubstitution(text="59152")) - return LaunchDescription([ - rsi_ip_address, - rsi_port, - Node( - package='kuka_rsi_simulator', - executable='rsi_simulator', - name='kuka_rsi_simulator', - parameters=[{ - 'rsi_ip_address': LaunchConfiguration('rsi_ip_address'), - 'rsi_port': LaunchConfiguration('rsi_port'), - }] - ) - ]) + return LaunchDescription( + [ + rsi_ip_address, + rsi_port, + Node( + package="kuka_rsi_simulator", + executable="rsi_simulator", + name="kuka_rsi_simulator", + parameters=[ + { + "rsi_ip_address": LaunchConfiguration("rsi_ip_address"), + "rsi_port": LaunchConfiguration("rsi_port"), + } + ], + ), + ] + ) diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml index d3b944b0..aeb5a03c 100644 --- a/kuka_rsi_simulator/package.xml +++ b/kuka_rsi_simulator/package.xml @@ -4,13 +4,9 @@ <name>kuka_rsi_simulator</name> <version>0.0.0</version> <description>Simple package for simulating the KUKA RSI interface</description> - <maintainer email="antal.marci@gmail.com">Marton Antal</maintainer> - <license>BSD</license> + <maintainer email="svastits1@gmail.com">Aron Svastits</maintainer> + <license>Apache 2.0</license> - <test_depend>ament_copyright</test_depend> - <test_depend>ament_flake8</test_depend> - <test_depend>ament_pep257</test_depend> - <test_depend>python3-pytest</test_depend> <exec_depend>ros2launch</exec_depend> <export> diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py index 21a11608..a06a7928 100644 --- a/kuka_rsi_simulator/setup.py +++ b/kuka_rsi_simulator/setup.py @@ -1,33 +1,42 @@ +# Copyright 2022 Márton Antal +# +# 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 setuptools import setup import os from glob import glob -package_name = 'kuka_rsi_simulator' +package_name = "kuka_rsi_simulator" setup( name=package_name, - version='0.1.0', + version="0.1.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), - glob(os.path.join('launch', '*.launch.py'))), - (os.path.join('share', package_name, 'launch'), - glob(os.path.join('launch', '*_launch.py'))), - (os.path.join('share', package_name, 'config'), - glob(os.path.join('config', '*.yaml'))), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ( + os.path.join("share", package_name, "launch"), + glob(os.path.join("launch", "*.launch.py")), + ), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Marton Antal', - maintainer_email='antal.marci@gmail.com', - description='Simple package for simulating the KUKA RSI interface.', - license='BSD', + maintainer="Marton Antal", + maintainer_email="antal.marci@gmail.com", + description="Simple package for simulating the KUKA RSI interface.", + license="BSD", entry_points={ - 'console_scripts': [ - 'rsi_simulator = kuka_rsi_simulator.rsi_simulator:main' - ], + "console_scripts": ["rsi_simulator = kuka_rsi_simulator.rsi_simulator:main"], }, ) diff --git a/kuka_rsi_simulator/test/test_copyright.py b/kuka_rsi_simulator/test/test_copyright.py deleted file mode 100644 index cc8ff03f..00000000 --- a/kuka_rsi_simulator/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/kuka_rsi_simulator/test/test_flake8.py b/kuka_rsi_simulator/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/kuka_rsi_simulator/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/kuka_rsi_simulator/test/test_pep257.py b/kuka_rsi_simulator/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/kuka_rsi_simulator/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index efdb2701..3354eeba 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -54,7 +54,7 @@ file(GLOB fri_client_sources src/fri_client_sdk/pb_frimessages_callbacks.c ) -# Add the Fast Robot Interface libray +# Add the Fast Robot Interface library add_library(fri_client_sdk SHARED ${fri_client_sources}) file(GLOB private_headers @@ -97,9 +97,13 @@ target_link_libraries(robot_manager_node pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) -install(TARGETS ${PROJECT_NAME} fri_connection fri_client_sdk robot_manager_node +install(TARGETS ${PROJECT_NAME} robot_manager_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS fri_connection fri_client_sdk configuration_manager + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) @@ -107,57 +111,8 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_pep257 REQUIRED) - find_package(ament_cmake_flake8 REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_xmllint REQUIRED) - - file(GLOB fri_client_includes - LIST_DIRECTORIES FALSE - RELATIVE "${PROJECT_SOURCE_DIR}" - include/fri_client_sdk/* - ) - - ament_copyright(--exclude ${fri_client_sources} ${private_headers} ${fri_client_includes}) - ament_cppcheck(--language=c++) - ament_pep257() - ament_flake8() - ament_cpplint(--exclude ${fri_client_sources} ${private_headers} ${fri_client_includes}) - ament_lint_cmake() - ament_uncrustify(--exclude ${fri_client_sources} ${private_headers} ${fri_client_includes}) - ament_xmllint() + find_package(launch_testing_ament_cmake) + add_launch_test(test/test_driver_startup.py) endif() ament_package() - -# ##################### -# # Project analysis ## -# ##################### - -# Additional options for project analysis -set(SONARQUBE_PACKAGES_FILE "none" CACHE STRING "Path to the file that contains the package source directories for SonarQube.") -option(TEST_COVERAGE "Generate test coverage reports upon testing." OFF) - -if(NOT ${SONARQUBE_PACKAGES_FILE} MATCHES "none") - file(APPEND ${SONARQUBE_PACKAGES_FILE} "${PROJECT_NAME};${PROJECT_SOURCE_DIR}\n") - message(${SONARQUBE_PACKAGES_FILE}) -endif() - -if(TEST_COVERAGE) - # Set build type to debug to avoid compiler optimization when checking coverage - set(CMAKE_BUILD_TYPE Debug) - - # Include coverage report related functions - list(APPEND CMAKE_MODULE_PATH "/usr/lib/cmake/CodeCoverage") - include(CodeCoverage) - - # Append compiler flags to gcc for generating coverage notes - append_coverage_compiler_flags() - - # Set coverage output dir to ${CMAKE_BINARY_DIR}/test_coverage - set_coverage_output_dir() -endif() diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 6a7151ce..a6ea0fca 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -7,4 +7,3 @@ robot_manager: send_period_ms: 10 joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] joint_stiffness: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] - diff --git a/kuka_sunrise_fri_driver/config/gpio_config.xacro b/kuka_sunrise_fri_driver/config/gpio_config.xacro index a8128761..0a2b45be 100755 --- a/kuka_sunrise_fri_driver/config/gpio_config.xacro +++ b/kuka_sunrise_fri_driver/config/gpio_config.xacro @@ -36,13 +36,13 @@ <state_interface name="Output1" data_type="boolean"/> <state_interface name="Output2" data_type="boolean"/> <state_interface name="Output3" data_type="boolean"/> - <state_interface name="Output4" data_type="boolean"/> + <state_interface name="Output4" data_type="boolean"/> <state_interface name="Output5" data_type="boolean"/> <state_interface name="Output6" data_type="boolean"/> - <state_interface name="Output7" data_type="boolean"/> + <state_interface name="Output7" data_type="boolean"/> <state_interface name="Output8" data_type="boolean"/> <state_interface name="Output9" data_type="boolean"/> - <state_interface name="Output10" data_type="boolean"/> + <state_interface name="Output10" data_type="boolean"/> </gpio> </xacro:macro> </robot> diff --git a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml index 55e008d5..ce5af6bb 100644 --- a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml @@ -1,6 +1,6 @@ joint_trajectory_controller: ros__parameters: - joints: + joints: - joint_1 - joint_2 - joint_3 @@ -16,4 +16,3 @@ joint_trajectory_controller: state_publish_rate: 50.0 action_monitor_rate: 20.0 allow_nonzero_velocity_at_trajectory_end: True - diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index 4dcd14bf..89ae9fd9 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -2,18 +2,12 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - forward_command_controller: - type: forward_command_controller/ForwardCommandController - joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - - effort_controller: - type: effort_controllers/JointEffortController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - + fri_configuration_controller: type: kuka_controllers/FRIConfigurationController diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h index 89084dae..8787bfad 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRCommand.h @@ -71,7 +71,7 @@ namespace FRI { /** - * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + * \brief Wrapper class for the FRI command message for a KUKA LBR (lightweight) robot. */ class LBRCommand { diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h index 2a049a4a..8e25b443 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friLBRState.h @@ -72,7 +72,7 @@ namespace FRI { /** - * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (leightweight) robot. + * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (lightweight) robot. */ class LBRState { diff --git a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h index 52452aa2..42ec2a0d 100644 --- a/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h +++ b/kuka_sunrise_fri_driver/include/fri_client_sdk/friTransformationClient.h @@ -174,7 +174,7 @@ class TransformationClient * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] * <p> * All provided transformation matrices need a timestamp that corresponds to their - * time of acquisiton. This timestamp must be synchronized to the timestamp + * time of acquisition. This timestamp must be synchronized to the timestamp * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). * <p> * If an update to the last transformation is not yet available when the provide() diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp index cfb6f429..5f287bed 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp @@ -16,18 +16,18 @@ #define KUKA_SUNRISE_FRI_DRIVER__CONFIGURATION_MANAGER_HPP_ #include <map> -#include <vector> #include <memory> #include <string> +#include <vector> +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "kuka_driver_interfaces/srv/set_int.hpp" +#include "kuka_sunrise_fri_driver/fri_connection.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" -#include "kuka_driver_interfaces/srv/set_int.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" -#include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp index dc87cccf..bca1c5c0 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp @@ -15,10 +15,10 @@ #ifndef KUKA_SUNRISE_FRI_DRIVER__FRI_CONNECTION_HPP_ #define KUKA_SUNRISE_FRI_DRIVER__FRI_CONNECTION_HPP_ -#include <memory> +#include <condition_variable> #include <functional> +#include <memory> #include <vector> -#include <condition_variable> namespace kuka_sunrise_fri_driver { @@ -27,29 +27,45 @@ class TCPConnection; enum CommandState : std::uint8_t { - ACCEPTED = 1, REJECTED = 2, UNKNOWN = 3, ERROR_CONTROL_ENDED = 4, ERROR_FRI_ENDED = 5 + ACCEPTED = 1, + REJECTED = 2, + UNKNOWN = 3, + ERROR_CONTROL_ENDED = 4, + ERROR_FRI_ENDED = 5 }; enum CommandID : std::uint8_t { - CONNECT = 1, DISCONNECT = 2, START_FRI = 3, END_FRI = 4, ACTIVATE_CONTROL = 5, - DEACTIVATE_CONTROL = 6, GET_FRI_CONFIG = 7, SET_FRI_CONFIG = 8, GET_CONTROL_MODE = 9, - SET_CONTROL_MODE = 10, GET_COMMAND_MODE = 11, SET_COMMAND_MODE = 12 + CONNECT = 1, + DISCONNECT = 2, + START_FRI = 3, + END_FRI = 4, + ACTIVATE_CONTROL = 5, + DEACTIVATE_CONTROL = 6, + GET_FRI_CONFIG = 7, + SET_FRI_CONFIG = 8, + GET_CONTROL_MODE = 9, + SET_CONTROL_MODE = 10, + GET_COMMAND_MODE = 11, + SET_COMMAND_MODE = 12 }; enum CommandSuccess : std::uint8_t { - SUCCESS = 1, NO_SUCCESS = 2 + SUCCESS = 1, + NO_SUCCESS = 2 }; enum ControlModeID : std::uint8_t { - POSITION_CONTROL_MODE = 1, JOINT_IMPEDANCE_CONTROL_MODE = 2 + POSITION_CONTROL_MODE = 1, + JOINT_IMPEDANCE_CONTROL_MODE = 2 }; enum ClientCommandModeID : std::uint8_t { - POSITION_COMMAND_MODE = 1, TORQUE_COMMAND_MODE = 3 + POSITION_COMMAND_MODE = 1, + TORQUE_COMMAND_MODE = 3 }; static const std::vector<std::uint8_t> FRI_CONFIG_HEADER = {0xAC, 0xED, 0x00, 0x05, 0x77, 0x0C}; @@ -70,8 +86,7 @@ class FRIConnection bool deactivateControl(); bool setPositionControlMode(); bool setJointImpedanceControlMode( - const std::vector<double> & joint_stiffness, - const std::vector<double> & joint_damping); + const std::vector<double> & joint_stiffness, const std::vector<double> & joint_damping); bool setClientCommandMode(ClientCommandModeID client_command_mode); // bool getControlMode(); bool setFRIConfig(int remote_port, int send_period_ms, int receive_multiplier); diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index e64d04da..3278388b 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -17,20 +17,20 @@ #include <condition_variable> #include <memory> -#include <vector> #include <string> #include <unordered_map> +#include <vector> -#include "rclcpp/rclcpp.hpp" #include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" #include "hardware_interface/system_interface.hpp" #include "kuka_driver_interfaces/srv/set_int.hpp" -#include "fri_client_sdk/friLBRClient.h" #include "fri_client_sdk/HWIFClientApplication.hpp" -#include "fri_client_sdk/friUdpConnection.h" #include "fri_client_sdk/friClientIf.h" +#include "fri_client_sdk/friLBRClient.h" +#include "fri_client_sdk/friUdpConnection.h" #include "kuka_sunrise_fri_driver/visibility_control.h" using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -45,24 +45,25 @@ enum class IOTypes BOOLEAN = 2, }; -static std::unordered_map<std::string, - IOTypes> const types = -{{"analog", IOTypes::ANALOG}, {"digital", IOTypes::DIGITAL}, {"boolean", IOTypes::BOOLEAN}}; +static std::unordered_map<std::string, IOTypes> const types = { + {"analog", IOTypes::ANALOG}, {"digital", IOTypes::DIGITAL}, {"boolean", IOTypes::BOOLEAN}}; class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, - public KUKA::FRI::LBRClient + public KUKA::FRI::LBRClient { public: RCLCPP_SHARED_PTR_DEFINITIONS(KukaFRIHardwareInterface) KUKA_SUNRISE_FRI_DRIVER_PUBLIC KukaFRIHardwareInterface() - : client_application_(udp_connection_, *this) {} - KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + : client_application_(udp_connection_, *this) + { + } + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn + on_init(const hardware_interface::HardwareInfo & info) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override; @@ -70,11 +71,9 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, export_command_interfaces() override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type read( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; + const rclcpp::Time & time, const rclcpp::Duration & period) override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type write( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; + const rclcpp::Time & time, const rclcpp::Duration & period) override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC void updateCommand(const rclcpp::Time & stamp); KUKA_SUNRISE_FRI_DRIVER_PUBLIC bool setReceiveMultiplier(int receive_multiplier); @@ -83,12 +82,13 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, class InvalidGPIOTypeException : public std::runtime_error { -public: + public: explicit InvalidGPIOTypeException(const std::string & gpio_type) : std::runtime_error( "GPIO type '" + gpio_type + "' is not supported, possible ones are 'analog', 'digital' or 'boolean'") - {} + { + } }; private: @@ -132,21 +132,29 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, KUKA_SUNRISE_FRI_DRIVER_LOCAL IOTypes getType(const std::string & type_string) const { auto it = types.find(type_string); - if (it != types.end()) { + if (it != types.end()) + { return it->second; - } else {throw InvalidGPIOTypeException(type_string);} + } + else + { + throw InvalidGPIOTypeException(type_string); + } } class GPIOReader { -public: - double & getData() {return data_;} - const std::string & getName() const {return name_;} + public: + double & getData() { return data_; } + const std::string & getName() const { return name_; } GPIOReader(const std::string & name, IOTypes type, const KUKA::FRI::LBRState & state) - : name_(name), type_(type), state_(state) {} + : name_(name), type_(type), state_(state) + { + } void getValue() { - switch (type_) { + switch (type_) + { case IOTypes::ANALOG: data_ = state_.getAnalogIOValue(name_.c_str()); break; @@ -159,7 +167,7 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, } } -private: + private: const std::string name_; IOTypes type_; const KUKA::FRI::LBRState & state_; @@ -168,16 +176,18 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, class GPIOWriter { -public: - double & getData() {return data_;} - const std::string & getName() const {return name_;} + public: + double & getData() { return data_; } + const std::string & getName() const { return name_; } GPIOWriter( - const std::string & name, IOTypes type, KUKA::FRI::LBRCommand & command, - double initial_value) - : name_(name), type_(type), command_(command), data_(initial_value) {} + const std::string & name, IOTypes type, KUKA::FRI::LBRCommand & command, double initial_value) + : name_(name), type_(type), command_(command), data_(initial_value) + { + } void setValue() { - switch (type_) { + switch (type_) + { case IOTypes::ANALOG: command_.setAnalogIOValue(name_.c_str(), data_); break; @@ -190,7 +200,7 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, } } -private: + private: const std::string name_; IOTypes type_; KUKA::FRI::LBRCommand & command_; diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 3361a5c8..657b06c3 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -19,17 +19,17 @@ #include <memory> #include <string> -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/client.hpp" -#include "std_srvs/srv/trigger.hpp" -#include "std_msgs/msg/bool.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/trigger.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" -#include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/configuration_manager.hpp" +#include "kuka_sunrise_fri_driver/fri_connection.hpp" namespace kuka_sunrise_fri_driver { @@ -39,17 +39,17 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode public: RobotManagerNode(); - virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &); + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &); - virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &); + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &); - virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &); + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &); - virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &); + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &); bool activateControl(); bool deactivateControl(); diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp index 7e37822e..6a510c9b 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp @@ -15,16 +15,16 @@ #ifndef KUKA_SUNRISE_FRI_DRIVER__TCP_CONNECTION_HPP_ #define KUKA_SUNRISE_FRI_DRIVER__TCP_CONNECTION_HPP_ -#include <sys/socket.h> #include <arpa/inet.h> -#include <unistd.h> #include <pthread.h> +#include <sys/socket.h> +#include <unistd.h> +#include <atomic> +#include <functional> #include <stdexcept> #include <string> -#include <functional> #include <vector> -#include <atomic> namespace kuka_sunrise_fri_driver { diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 46673079..d4b8b5d7 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -22,7 +22,7 @@ def launch_setup(context, *args, **kwargs): - robot_model = LaunchConfiguration('robot_model') + robot_model = LaunchConfiguration("robot_model") # Get URDF via xacro robot_description_content = Command( @@ -30,46 +30,58 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare("kuka_lbr_iiwa_support"), - "urdf", robot_model.perform(context) + ".urdf.xacro"] + [ + FindPackageShare("kuka_lbr_iiwa_support"), + "urdf", + robot_model.perform(context) + ".urdf.xacro", + ] ), " ", - ], on_stderr='capture' + ], + on_stderr="capture", ) # Get URDF via xacro - robot_description = {'robot_description': robot_description_content} + robot_description = {"robot_description": robot_description_content} - controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/ros2_controller_config.yaml") + controller_config = ( + get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/ros2_controller_config.yaml" + ) - joint_traj_controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/joint_trajectory_controller_config.yaml") + joint_traj_controller_config = ( + get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/joint_trajectory_controller_config.yaml" + ) - driver_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/driver_config.yaml") + driver_config = ( + get_package_share_directory("kuka_sunrise_fri_driver") + "/config/driver_config.yaml" + ) - controller_manager_node = '/controller_manager' + controller_manager_node = "/controller_manager" control_node = Node( - package='kuka_drivers_core', - executable='control_node', - parameters=[robot_description, controller_config] + package="kuka_drivers_core", + executable="control_node", + parameters=[robot_description, controller_config], ) robot_manager_node = LifecycleNode( - name=['robot_manager'], - namespace='', + name=["robot_manager"], + namespace="", package="kuka_sunrise_fri_driver", executable="robot_manager_node", - parameters=[driver_config, {'robot_model': robot_model.perform(context)}, - {'position_controller_name': 'joint_trajectory_controller'}, - {'torque_controller_name': ''}] + parameters=[ + driver_config, + {"robot_model": robot_model.perform(context)}, + {"position_controller_name": "joint_trajectory_controller"}, + {"torque_controller_name": ""}, + ], ) robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], ) # Spawn controllers @@ -77,24 +89,31 @@ def controller_spawner(controller_with_config): return Node( package="controller_manager", executable="spawner", - arguments=[controller_with_config[0], "-c", controller_manager_node, "-p", - controller_with_config[1], "--inactive"] + arguments=[ + controller_with_config[0], + "-c", + controller_manager_node, + "-p", + controller_with_config[1], + "--inactive", + ], ) controller_names_and_config = [ ("joint_state_broadcaster", []), ("joint_trajectory_controller", joint_traj_controller_config), ("fri_configuration_controller", []), - ("fri_state_broadcaster", []) + ("fri_state_broadcaster", []), ] - controller_spawners = [controller_spawner(controllers) - for controllers in controller_names_and_config] + controller_spawners = [ + controller_spawner(controllers) for controllers in controller_names_and_config + ] nodes_to_start = [ control_node, robot_manager_node, - robot_state_publisher + robot_state_publisher, ] + controller_spawners return nodes_to_start @@ -102,8 +121,5 @@ def controller_spawner(controller_with_config): def generate_launch_description(): launch_arguments = [] - launch_arguments.append(DeclareLaunchArgument( - 'robot_model', - default_value='lbr_iiwa14_r820' - )) + launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iiwa14_r820")) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py b/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py index c16a3f4a..42d0f4b1 100644 --- a/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py @@ -19,24 +19,31 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch.actions.include_launch_description import IncludeLaunchDescription -from launch.launch_description_sources.python_launch_description_source import PythonLaunchDescriptionSource # noqa: E501 +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 def generate_launch_description(): rviz_config_file = os.path.join( - get_package_share_directory('kuka_resources'), 'config', 'view_7_axis_urdf.rviz') + get_package_share_directory("kuka_resources"), "config", "view_7_axis_urdf.rviz" + ) - startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( - [get_package_share_directory('kuka_sunrise_fri_driver'), '/launch/startup.launch.py'])) - - return LaunchDescription([ - startup_launch, - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file, - "--ros-args", "--log-level", "error"], + startup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory("kuka_sunrise_fri_driver"), "/launch/startup.launch.py"] ) - ]) + ) + + return LaunchDescription( + [ + startup_launch, + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, "--ros-args", "--log-level", "error"], + ), + ] + ) diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index d9b6a9ed..2c032a9f 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -23,16 +23,8 @@ <depend>controller_manager_msgs</depend> <exec_depend>rosidl_default_runtime</exec_depend> - <exec_depend>ros2_control</exec_depend> - <exec_depend>ros2_controllers</exec_depend> - - <test_depend>ament_cmake_copyright</test_depend> - <test_depend>ament_cmake_cppcheck</test_depend> - <test_depend>ament_cmake_pep257</test_depend> - <test_depend>ament_cmake_flake8</test_depend> - <test_depend>ament_cmake_cpplint</test_depend> - <test_depend>ament_cmake_lint_cmake</test_depend> - <test_depend>ament_cmake_xmllint</test_depend> + <exec_depend>ros2_control</exec_depend> + <exec_depend>ros2_controllers</exec_depend> <export> <build_type>ament_cmake</build_type> diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java index 8ed79a1c..447b2f94 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java @@ -12,16 +12,16 @@ /** * Implementation of a robot application. * <p> - * The application provides a {@link RoboticsAPITask#initialize()} and a - * {@link RoboticsAPITask#run()} method, which will be called successively in - * the application lifecycle. The application will terminate automatically after - * the {@link RoboticsAPITask#run()} method has finished or after stopping the - * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an - * exception is thrown during initialization or run. + * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. * <p> - * <b>It is imperative to call <code>super.dispose()</code> when overriding the - * {@link RoboticsAPITask#dispose()} method.</b> - * + * <b>It is imperative to call <code>super.dispose()</code> when overriding the + * {@link RoboticsAPITask#dispose()} method.</b> + * * @see UseRoboticsAPIContext * @see #initialize() * @see #run() @@ -30,7 +30,7 @@ public class ROS2_Control extends RoboticsAPIApplication { @Inject private LBR _lbr; - + private TCPConnection _TCPConnection; private ROS2Connection _ROS2Connection; private FRIManager _FRIManager; @@ -41,7 +41,7 @@ public void initialize() { _TCPConnection = new TCPConnection(30000); _ROS2Connection = new ROS2Connection(); _FRIManager = new FRIManager(_lbr, getApplicationControl()); - + _FRIManager.registerROS2ConnectionModule(_ROS2Connection); _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); _ROS2Connection.registerTCPConnectionModule(_TCPConnection); @@ -57,7 +57,7 @@ public void run() { _ROS2Connection.rejectCommands(); _FRIManager.close(); } - + @Override public void dispose() { getLogger().info("disposes"); diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java index 1e0898fb..299b9362 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java @@ -32,7 +32,7 @@ public enum CommandResult{ ERRORED; } private ROS2Connection _ROS2Connection; - + private State _currentState; private LBR _lbr; private FRISession _FRISession; @@ -42,9 +42,9 @@ public enum CommandResult{ private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); //private IApplicationControl _applicationControl; - + private static double[] stiffness_ = new double[7]; - + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); _lbr = lbr; @@ -57,11 +57,11 @@ public FRIManager(LBR lbr, IApplicationControl applicationControl){ //_applicationControl = applicationControl; applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); } - + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ _ROS2Connection = ros2ConnectionModule; } - + public void close(){ if(_currentState instanceof ControlActiveState){ deactivateControl(); @@ -70,36 +70,36 @@ public void close(){ endFRI();//TODO: handle error } } - + public FRIConfigurationParams getFRIConfig(){ FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); return friConfigurationParams; } - + public ClientCommandMode getClientCommandMode(){ return _clientCommandMode; } - + public IMotionControlMode getControlMode(){ return _controlMode; } - + public FRISessionState getFRISessionState(){ return _FRISession.getFRIChannelInformation().getFRISessionState(); } - + /* * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation * */ - + public CommandResult setControlMode(IMotionControlMode controlMode){ return _currentState.setControlMode(controlMode); } - + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return _currentState.setCommandMode(clientCommandMode); } - + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ return _currentState.setFRIConfig(friConfigurationParams); } @@ -111,7 +111,7 @@ public CommandResult startFRI(){ } return commandResult; } - + public CommandResult endFRI(){ CommandResult commandResult = _currentState.endFRI(); if(commandResult == CommandResult.EXECUTED){ @@ -119,7 +119,7 @@ public CommandResult endFRI(){ } return commandResult; } - + public CommandResult activateControl(){ CommandResult commandResult = _currentState.activateControl(); if(commandResult == CommandResult.EXECUTED){ @@ -127,7 +127,7 @@ public CommandResult activateControl(){ } return commandResult; } - + public CommandResult deactivateControl(){ CommandResult commandResult = _currentState.deactivateControl(); if(commandResult == CommandResult.EXECUTED){ @@ -135,38 +135,38 @@ public CommandResult deactivateControl(){ } return commandResult; } - + private class State{ /* By default reject all commands */ public CommandResult startFRI(){ return CommandResult.REJECTED; } - + public CommandResult endFRI(){ return CommandResult.REJECTED; } - + public CommandResult activateControl(){ return CommandResult.REJECTED; } - + public CommandResult deactivateControl(){ return CommandResult.REJECTED; } - + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ return CommandResult.REJECTED; } - + public CommandResult setControlMode(IMotionControlMode controlMode){ return CommandResult.REJECTED; } - + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.REJECTED; } } - + private class InactiveState extends State{ @Override public CommandResult startFRI(){ @@ -177,7 +177,7 @@ public CommandResult startFRI(){ FRIManager.this._FRISession.close(); return CommandResult.ERRORED; } - + return CommandResult.EXECUTED; } @Override @@ -196,7 +196,7 @@ public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.EXECUTED; } } - + private class FRIActiveState extends State { @Override public CommandResult endFRI(){ @@ -209,12 +209,12 @@ public CommandResult endFRI(){ } @Override public CommandResult activateControl(){ - FRIJointOverlay friJointOverlay = + FRIJointOverlay friJointOverlay = new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); //friJointOverlay.overrideJointAcceleration(20.0); - PositionHold motion = + PositionHold motion = new PositionHold(FRIManager.this._controlMode, -1, null); - FRIManager.this._motionContainer = + FRIManager.this._motionContainer = FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); return CommandResult.EXECUTED; } @@ -229,7 +229,7 @@ public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.EXECUTED; } } - + private class ControlActiveState extends State { @Override public CommandResult deactivateControl(){ @@ -237,7 +237,7 @@ public CommandResult deactivateControl(){ return CommandResult.EXECUTED; } } - + private class FRIMotionErrorHandler implements IErrorHandler{ @Override @@ -261,11 +261,7 @@ public ErrorHandlingAction handleError(Device device, System.out.println("Cancelled containers: " + canceledContainers.toString()); return ErrorHandlingAction.Ignore; } - - } - -} - - + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java index bc6f05a9..be495f8d 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java @@ -20,37 +20,37 @@ public class ROS2Connection { private FRIManager _FRIManager; private boolean _acceptingCommands = false; private boolean _disconnect = false; - + public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ _TCPConnection = tcpConnectionModule; } - + public void registerFRIManagerModule(FRIManager friManagerModule){ _FRIManager = friManagerModule; } - + public void acceptCommands(){ _acceptingCommands = true; } - + public void rejectCommands(){ _acceptingCommands = false; } - + private enum CommandState{ ACCEPTED((byte)1), REJECTED((byte)2), UNKNOWN((byte)3), ERROR_CONTROL_ENDED((byte)4), ERROR_FRI_ENDED((byte)5); - + private final byte value; - + private CommandState(byte value){ this.value = value; } } - + private enum CommandID{ CONNECT( (byte)1), DISCONNECT( (byte)2), @@ -64,14 +64,14 @@ private enum CommandID{ SET_CONTROL_MODE( (byte)10), GET_COMMAND_MODE( (byte)11), SET_COMMAND_MODE( (byte)12); - + private final byte value; - + private CommandID(byte value){ this.value = value; - } + } + - public static CommandID fromByte(byte value){ for(CommandID id : CommandID.values()){ if(value == id.value){ @@ -81,28 +81,28 @@ public static CommandID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); } } - + private enum SuccessSignalID { SUCCESS((byte)1), NO_SUCCESS((byte)2); - + private final byte value; - + private SuccessSignalID(byte value){ this.value = value; } } - + private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2); - + public final byte value; - + ControlModeID(byte value){ this.value = value; } - + public static ControlModeID fromByte(byte value){ for(ControlModeID id : ControlModeID.values()){ if(value == id.value){ @@ -112,7 +112,7 @@ public static ControlModeID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); } } - + public void handleMessageFromROS(byte[] inMessage){ CommandID command = null; try{ @@ -122,18 +122,18 @@ public void handleMessageFromROS(byte[] inMessage){ feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); return; } - + } catch(RuntimeException e){ System.out.println(e.getMessage()); feedbackCommandUnknown(e.getMessage().getBytes()); return; } - + try{ byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); byte[] feedbackData = null; System.out.println("Command received: " + command.toString()); - + switch(command){ case CONNECT: feedbackData = connect(inMessageData); @@ -181,9 +181,9 @@ public void handleMessageFromROS(byte[] inMessage){ System.out.println(e.getMessage()); feedbackCommandNoSuccess(command, e.getMessage().getBytes()); return; - } + } } - + public void handleConnectionLost(){ _FRIManager.deactivateControl(); _FRIManager.endFRI(); @@ -196,38 +196,38 @@ public void handleControlEndedError(){ System.out.println("Error: control ended"); _TCPConnection.sendBytes(message); } - + public void handleFRIEndedError(){ byte[] message = {CommandState.ERROR_FRI_ENDED.value}; System.out.println("Error: session ended"); _TCPConnection.sendBytes(message); } - + private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ byte[] message = appendByte(command.value, feedbackData); message = appendByte(CommandState.REJECTED.value, message); _TCPConnection.sendBytes(message); } - + private void feedbackCommandUnknown(byte[] feedbackData){ byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); _TCPConnection.sendBytes(message); } - + private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); message = appendByte(command.value, message); message = appendByte(CommandState.ACCEPTED.value, message); _TCPConnection.sendBytes(message); } - + private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); message = appendByte(command.value, message); message = appendByte(CommandState.ACCEPTED.value, message); _TCPConnection.sendBytes(message); } - + private byte[] appendByte(byte id, byte[] data){ byte[] message = null; if(data == null){ @@ -239,18 +239,18 @@ private byte[] appendByte(byte id, byte[] data){ } return message; } - + private byte[] connect(byte[] cmdData){ return null; } - + private byte[] disconnect(byte[] cmdData){ _FRIManager.close(); _disconnect = true; //_TCPConnection.closeConnection(); //TODO: close connection after feedback was sent return null; } - + private byte[] startFRI(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.startFRI(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -261,7 +261,7 @@ private byte[] startFRI(byte[] cmdData){ } return null; } - + private byte[] endFRI(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.endFRI(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -272,7 +272,7 @@ private byte[] endFRI(byte[] cmdData){ } return null; } - + private byte[] activateControl(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.activateControl(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -283,7 +283,7 @@ private byte[] activateControl(byte[] cmdData){ } return null; } - + private byte[] deactivateControl(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -294,13 +294,13 @@ private byte[] deactivateControl(byte[] cmdData){ } return null; } - + private byte[] getFRIConfig(byte[] cmdData){ FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); return feedbackData; } - + private byte[] setFRIConfig(byte[] cmdData) { ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); @@ -314,7 +314,7 @@ private byte[] setFRIConfig(byte[] cmdData) { } return null; } - + private byte[] getControlMode(byte[] cmdData){ IMotionControlMode controlMode = _FRIManager.getControlMode(); ControlModeID controlModeID; @@ -331,13 +331,13 @@ private byte[] getControlMode(byte[] cmdData){ byte[] message = appendByte(controlModeID.value, controlModeData); return message; } - + private byte[] setControlMode(byte[] cmdData){ ensureArrayLength(cmdData, 1); ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); - + byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); - + IMotionControlMode controlMode = null; switch(controlModeID){ case POSITION: @@ -361,14 +361,14 @@ private byte[] setControlMode(byte[] cmdData){ } return null; } - + private byte[] getCommandMode(byte[] cmdData){ ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); byte[] commandModeData = new byte[1]; commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value return commandModeData; } - + private byte[] setCommandMode(byte[] cmdData){ ensureArrayLength(cmdData, 1); ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); @@ -384,7 +384,7 @@ private byte[] setCommandMode(byte[] cmdData){ } return null; } - + private void ensureArrayLength(byte[] array, int requiredLength){ if(array == null){ throw new RuntimeException("Array is null"); @@ -393,5 +393,5 @@ private void ensureArrayLength(byte[] array, int requiredLength){ throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); } } - -} \ No newline at end of file + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java index d409de16..58ec404e 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java @@ -15,7 +15,7 @@ public class TCPConnection{ private boolean _closeRequested; private byte[] _incomingData; private ROS2Connection _ROS2Connection; - + public TCPConnection(int tcpServerPort){ _tcpServerPort = tcpServerPort; _tcpServer = null; @@ -24,11 +24,11 @@ public TCPConnection(int tcpServerPort){ _closeRequested = false; _incomingData = null; } - + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ _ROS2Connection = ros2ConnectionModule; } - + public void waitUntilDisconnected(){ try { _tcpConnectionThread.join(); @@ -37,7 +37,7 @@ public void waitUntilDisconnected(){ e.printStackTrace(); } } - + private class ConnectionHandler implements Runnable{ public void run(){ try{ @@ -69,7 +69,7 @@ public void run(){ _closeRequested = false; } } - + public void openConnection() { try { _tcpServer = new ServerSocket(_tcpServerPort); @@ -79,7 +79,7 @@ public void openConnection() { } _tcpConnectionThread.start(); } - + public void sendString(String s){ if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ try{ @@ -94,7 +94,7 @@ public void sendString(String s){ System.out.println("TCP client not connected."); } } - + public void sendBytes(byte[] message){ if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ try{ @@ -108,13 +108,13 @@ public void sendBytes(byte[] message){ System.out.println("TCP client not connected."); } } - + public byte[] getReceivedData(){ byte[] dataCopy = _incomingData; _incomingData = null; return dataCopy; } - + public void closeConnection(){ _closeRequested = true; try{ @@ -130,7 +130,7 @@ public void closeConnection(){ e.printStackTrace(); } } - + private void waitForConnection() throws Exception { System.out.println("Waiting for connection..."); try { @@ -143,10 +143,10 @@ private void waitForConnection() throws Exception { throw e; } - } + } System.out.println("Connection established."); } - + private void handleIncomingData() throws IOException{ BufferedReader inFromClient = new BufferedReader(new InputStreamReader(_tcpClient.getInputStream())); while(_tcpClient.isClosed() == false){ @@ -169,7 +169,7 @@ private void handleIncomingData() throws IOException{ byte[] byteArray = Arrays.copyOf(new String(charArray).getBytes(), dataLength); String byteHexString = DatatypeConverter.printHexBinary(byteArray); - + if(_incomingData != null){ System.out.println("ERROR: Previous data not yet processed! Skipping data: " + byteHexString); } @@ -181,8 +181,7 @@ private void handleIncomingData() throws IOException{ } } } - - - -} + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java index d1e3cdec..e308c9c0 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java @@ -13,17 +13,17 @@ public abstract class ControlModeParams implements Externalizable{ public static int length = 0; - + private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2); - + public final byte value; - + ControlModeID(byte value){ this.value = value; } - + public static ControlModeID fromByte(byte value){ for(ControlModeID id : ControlModeID.values()){ if(value == id.value){ @@ -33,7 +33,7 @@ public static ControlModeID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); } } - + public static ControlModeParams fromSerialData(byte[] serialData){ if(serialData.length == 0){ throw new RuntimeException("serialData is empty"); @@ -52,7 +52,7 @@ public static ControlModeParams fromSerialData(byte[] serialData){ MessageEncoding.Decode(serialData, controlModeParams); return controlModeParams; } - + public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ if(controlMode == null){ throw new RuntimeException("ControlMode is null"); @@ -67,19 +67,19 @@ public static ControlModeParams fromMotionControlMode(IMotionControlMode control } return controlModeParams; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { - - + + } @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - + } - + } class PositionControlModeParams extends ControlModeParams{ @@ -89,9 +89,9 @@ class PositionControlModeParams extends ControlModeParams{ class JointImpedanceControlModeParams extends ControlModeParams{ public JointImpedanceControlModeParams(){ - + } public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ - + } } diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java index 3dd41f45..683a6bce 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java @@ -10,13 +10,13 @@ public class FRIConfigurationParams implements Externalizable { public static final int length = 12; //remoteIP not included - - + + private String _remoteIP = "192.168.37.70"; private int _remotePort; private int _sendPeriodMilliSec; private int _receiveMultiplier; - + @Override public void writeExternal(ObjectOutput out) throws IOException { //out.writeBytes(_remoteIP); @@ -33,20 +33,20 @@ public void readExternal(ObjectInput in) throws IOException, _sendPeriodMilliSec = in.readInt(); _receiveMultiplier = in.readInt(); } - + public FRIConfigurationParams() { _remotePort = 30200; _sendPeriodMilliSec = 10; _receiveMultiplier = 1; } - + public FRIConfigurationParams(FRIConfiguration friConfiguration){ //_remoteIP = friConfiguration.getHostName(); _remotePort = friConfiguration.getPortOnRemote(); _sendPeriodMilliSec = friConfiguration.getSendPeriodMilliSec(); _receiveMultiplier = friConfiguration.getReceiveMultiplier(); } - + public FRIConfiguration toFRIConfiguration(Device device){ FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(device, _remoteIP); friConfiguration.setPortOnRemote(_remotePort); diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java index 371f9e67..bd933b6c 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java @@ -13,20 +13,20 @@ public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ public final static int length = 112; - + public JointImpedanceControlModeExternalizable(){ super(1000, 1000, 1000, 1000, 1000, 1000, 1000); } - + public JointImpedanceControlModeExternalizable(JointImpedanceControlMode other){ super(other); } - + public IMotionControlMode toControlMode(){ JointImpedanceControlMode controlMode = new JointImpedanceControlMode((JointImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double jointStiffness : getStiffness()){ @@ -40,18 +40,18 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] jointStiffness = new double[getStiffness().length]; + double[] jointStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ jointStiffness[i] = in.readDouble(); } setStiffness(jointStiffness); - - double[] jointDamping = new double[getDamping().length]; + + double[] jointDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ jointDamping[i] = in.readDouble(); } setDamping(jointDamping); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java index 286ba286..13d77813 100644 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java +++ b/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java @@ -22,7 +22,7 @@ public static byte[] Encode(Externalizable objectIn, int serialLength){ } return serialDataOut; } - + public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws RuntimeException{ try{ ByteArrayInputStream serialDataStream = new ByteArrayInputStream(serialDataIn); @@ -31,7 +31,7 @@ public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws objectStream.close(); } catch(IOException e){ e.printStackTrace(); - throw new RuntimeException("IO Exception occured"); + throw new RuntimeException("IO Exception occurred"); } catch (ClassNotFoundException e) { throw new RuntimeException("Message could not be decoded"); } diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp index 1c466c39..c2bcd467 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp @@ -16,8 +16,8 @@ #include <string> #include <vector> -#include "kuka_sunrise_fri_driver/configuration_manager.hpp" #include "communication_helpers/service_tools.hpp" +#include "kuka_sunrise_fri_driver/configuration_manager.hpp" namespace kuka_sunrise_fri_driver { @@ -28,56 +28,63 @@ ConfigurationManager::ConfigurationManager( { auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); - cbg_ = robot_manager_node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - param_cbg_ = robot_manager_node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - receive_multiplier_client_ = robot_manager_node->create_client< - kuka_driver_interfaces::srv::SetInt>( - "set_receive_multiplier", qos.get_rmw_qos_profile(), - cbg_); + cbg_ = robot_manager_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + param_cbg_ = + robot_manager_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + receive_multiplier_client_ = + robot_manager_node->create_client<kuka_driver_interfaces::srv::SetInt>( + "set_receive_multiplier", qos.get_rmw_qos_profile(), cbg_); robot_manager_node_->registerParameter<std::string>( - "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights {false, false, - false, false, true}, [this](const std::string & controller_ip) { - return this->onControllerIpChangeRequest(controller_ip); - }); + "controller_ip", "", + kuka_drivers_core::ParameterSetAccessRights{false, false, false, false, true}, + [this](const std::string & controller_ip) + { return this->onControllerIpChangeRequest(controller_ip); }); set_parameter_service_ = robot_manager_node->create_service<std_srvs::srv::Trigger>( - "configuration_manager/set_params", [this]( + "configuration_manager/set_params", + [this]( std_srvs::srv::Trigger::Request::SharedPtr, - std_srvs::srv::Trigger::Response::SharedPtr response) { - this->setParameters(response); - }, ::rmw_qos_profile_default, param_cbg_); + std_srvs::srv::Trigger::Response::SharedPtr response) { this->setParameters(response); }, + ::rmw_qos_profile_default, param_cbg_); get_controllers_client_ = robot_manager_node->create_client<controller_manager_msgs::srv::ListControllers>( - "controller_manager/list_controllers", qos.get_rmw_qos_profile(), cbg_); + "controller_manager/list_controllers", qos.get_rmw_qos_profile(), cbg_); } bool ConfigurationManager::onCommandModeChangeRequest(const std::string & command_mode) const { - if (command_mode == POSITION_COMMAND) { - if (!position_controller_available_ || !setCommandMode(POSITION_COMMAND)) { + if (command_mode == POSITION_COMMAND) + { + if (!position_controller_available_ || !setCommandMode(POSITION_COMMAND)) + { return false; } - } else if (command_mode == TORQUE_COMMAND) { - if (robot_manager_node_->get_parameter("control_mode").as_string() != IMPEDANCE_CONTROL) { + } + else if (command_mode == TORQUE_COMMAND) + { + if (robot_manager_node_->get_parameter("control_mode").as_string() != IMPEDANCE_CONTROL) + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Unable to set torque command mode, if control mode is not 'joint impedance'"); return false; } - if (robot_manager_node_->get_parameter("send_period_ms").as_int() > 5) { + if (robot_manager_node_->get_parameter("send_period_ms").as_int() > 5) + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Unable to set torque command mode, if send period is bigger than 5 [ms]"); return false; } - if (!torque_controller_available_ || !setCommandMode(TORQUE_COMMAND)) { + if (!torque_controller_available_ || !setCommandMode(TORQUE_COMMAND)) + { return false; } - } else { + } + else + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Command mode should be '%s' or '%s'", POSITION_COMMAND.c_str(), TORQUE_COMMAND.c_str()); @@ -89,18 +96,24 @@ bool ConfigurationManager::onCommandModeChangeRequest(const std::string & comman bool ConfigurationManager::onControlModeChangeRequest(const std::string & control_mode) const { - if (control_mode == POSITION_CONTROL) { + if (control_mode == POSITION_CONTROL) + { return fri_connection_->setPositionControlMode(); - } else if (control_mode == IMPEDANCE_CONTROL) { - try { - return fri_connection_->setJointImpedanceControlMode( - joint_stiffness_, - joint_damping_); - } catch (const std::exception & e) { + } + else if (control_mode == IMPEDANCE_CONTROL) + { + try + { + return fri_connection_->setJointImpedanceControlMode(joint_stiffness_, joint_damping_); + } + catch (const std::exception & e) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), e.what()); } return false; - } else { + } + else + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Control mode should be '%s' or '%s'", POSITION_CONTROL.c_str(), IMPEDANCE_CONTROL.c_str()); @@ -111,16 +124,18 @@ bool ConfigurationManager::onControlModeChangeRequest(const std::string & contro bool ConfigurationManager::onJointStiffnessChangeRequest( const std::vector<double> & joint_stiffness) { - if (joint_stiffness.size() != 7) { + if (joint_stiffness.size() != 7) + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Invalid parameter array length for parameter joint stiffness"); return false; } - for (double js : joint_stiffness) { - if (js < 0) { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Joint stiffness values must be >=0"); + for (double js : joint_stiffness) + { + if (js < 0) + { + RCLCPP_ERROR(robot_manager_node_->get_logger(), "Joint stiffness values must be >=0"); return false; } } @@ -130,14 +145,17 @@ bool ConfigurationManager::onJointStiffnessChangeRequest( bool ConfigurationManager::onJointDampingChangeRequest(const std::vector<double> & joint_damping) { - if (joint_damping.size() != 7) { + if (joint_damping.size() != 7) + { RCLCPP_ERROR( robot_manager_node_->get_logger(), "Invalid parameter array length for parameter joint damping"); return false; } - for (double jd : joint_damping) { - if (jd < 0 || jd > 1) { + for (double jd : joint_damping) + { + if (jd < 0 || jd > 1) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Joint damping values must be >=0 && <=1"); return false; } @@ -148,10 +166,10 @@ bool ConfigurationManager::onJointDampingChangeRequest(const std::vector<double> bool ConfigurationManager::onSendPeriodChangeRequest(const int & send_period) const { - if (send_period < 1 || send_period > 100) { + if (send_period < 1 || send_period > 100) + { RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Send period milliseconds must be >=1 && <=100"); + robot_manager_node_->get_logger(), "Send period milliseconds must be >=1 && <=100"); return false; } return true; @@ -159,11 +177,13 @@ bool ConfigurationManager::onSendPeriodChangeRequest(const int & send_period) co bool ConfigurationManager::onReceiveMultiplierChangeRequest(const int & receive_multiplier) const { - if (receive_multiplier < 1) { + if (receive_multiplier < 1) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Receive multiplier must be >=1"); return false; } - if (!setReceiveMultiplier(receive_multiplier)) { + if (!setReceiveMultiplier(receive_multiplier)) + { return false; } return true; @@ -175,25 +195,28 @@ bool ConfigurationManager::onControllerIpChangeRequest(const std::string & contr size_t i = 0; std::vector<std::string> split_ip; auto pos = controller_ip.find('.'); - while (pos != std::string::npos) { + while (pos != std::string::npos) + { split_ip.push_back(controller_ip.substr(i, pos - i)); i = ++pos; pos = controller_ip.find('.', pos); } split_ip.push_back(controller_ip.substr(i, controller_ip.length())); - if (split_ip.size() != 4) { + if (split_ip.size() != 4) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Valid IP must have 3 '.' delimiters"); return false; } - for (const auto & ip : split_ip) { - if (ip.empty() || (ip.find_first_not_of("[0123456789]") != std::string::npos) || - stoi(ip) > 255 || stoi(ip) < 0) + for (const auto & ip : split_ip) + { + if ( + ip.empty() || (ip.find_first_not_of("[0123456789]") != std::string::npos) || stoi(ip) > 255 || + stoi(ip) < 0) { RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Valid IP must contain only numbers between 0 and 255"); + robot_manager_node_->get_logger(), "Valid IP must contain only numbers between 0 and 255"); return false; } } @@ -201,32 +224,45 @@ bool ConfigurationManager::onControllerIpChangeRequest(const std::string & contr } bool ConfigurationManager::onControllerNameChangeRequest( - const std::string & controller_name, - bool position) + const std::string & controller_name, bool position) { auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>(); auto response = kuka_drivers_core::sendRequest<controller_manager_msgs::srv::ListControllers::Response>( - get_controllers_client_, request, 0, 1000); + get_controllers_client_, request, 0, 1000); - if (!response) { + if (!response) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Could not get controller names"); return false; } - if (controller_name == "") { + if (controller_name == "") + { RCLCPP_WARN( robot_manager_node_->get_logger(), "Controller for %s command mode not available", position ? POSITION_COMMAND.c_str() : TORQUE_COMMAND.c_str()); - if (position) {position_controller_available_ = false;} else { + if (position) + { + position_controller_available_ = false; + } + else + { torque_controller_available_ = false; } return true; } - for (const auto & controller : response->controller) { - if (controller_name == controller.name) { - if (position) {position_controller_available_ = true;} else { + for (const auto & controller : response->controller) + { + if (controller_name == controller.name) + { + if (position) + { + position_controller_available_ = true; + } + else + { torque_controller_available_ = true; } return true; @@ -241,17 +277,25 @@ bool ConfigurationManager::onControllerNameChangeRequest( bool ConfigurationManager::setCommandMode(const std::string & command_mode) const { ClientCommandModeID client_command_mode; - if (command_mode == POSITION_COMMAND) { + if (command_mode == POSITION_COMMAND) + { client_command_mode = POSITION_COMMAND_MODE; - } else if (command_mode == TORQUE_COMMAND) { + } + else if (command_mode == TORQUE_COMMAND) + { client_command_mode = TORQUE_COMMAND_MODE; - } else { + } + else + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Invalid control mode"); return false; } - if (fri_connection_) { + if (fri_connection_) + { fri_connection_->setClientCommandMode(client_command_mode); - } else { + } + else + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Robot Manager not available"); return false; } @@ -266,7 +310,8 @@ bool ConfigurationManager::setReceiveMultiplier(int receive_multiplier) const auto response = kuka_drivers_core::sendRequest<kuka_driver_interfaces::srv::SetInt::Response>( receive_multiplier_client_, request, 0, 1000); - if (!response || !response->success) { + if (!response || !response->success) + { RCLCPP_ERROR(robot_manager_node_->get_logger(), "Could not set receive_multiplier"); return false; } @@ -275,7 +320,8 @@ bool ConfigurationManager::setReceiveMultiplier(int receive_multiplier) const void ConfigurationManager::setParameters(std_srvs::srv::Trigger::Response::SharedPtr response) { - if (configured_) { + if (configured_) + { RCLCPP_WARN(robot_manager_node_->get_logger(), "Parameters already registered"); response->success = true; return; @@ -286,55 +332,51 @@ void ConfigurationManager::setParameters(std_srvs::srv::Trigger::Response::Share // parameter type (or value), the nodes must be launched again with changed parameters // because they could not be declared, therefore change is not possible in runtime robot_manager_node_->registerParameter<int>( - "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights {false, true, false, false, - true}, [this](const int & send_period) { - return this->onSendPeriodChangeRequest(send_period); - }); + "send_period_ms", 10, + kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, + [this](const int & send_period) { return this->onSendPeriodChangeRequest(send_period); }); robot_manager_node_->registerParameter<std::vector<double>>( - "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights {false, - true, true, false, true}, [this](const std::vector<double> & joint_stiffness) { - return this->onJointStiffnessChangeRequest(joint_stiffness); - }); + "joint_stiffness", joint_stiffness_, + kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, + [this](const std::vector<double> & joint_stiffness) + { return this->onJointStiffnessChangeRequest(joint_stiffness); }); robot_manager_node_->registerParameter<std::vector<double>>( - "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights {false, true, - true, false, true}, [this](const std::vector<double> & joint_damping) { - return this->onJointDampingChangeRequest(joint_damping); - }); + "joint_damping", joint_damping_, + kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, + [this](const std::vector<double> & joint_damping) + { return this->onJointDampingChangeRequest(joint_damping); }); robot_manager_node_->registerParameter<std::string>( - "control_mode", POSITION_CONTROL, kuka_drivers_core::ParameterSetAccessRights {false, true, - true, - false, true}, [this](const std::string & control_mode) { - return this->onControlModeChangeRequest(control_mode); - }); + "control_mode", POSITION_CONTROL, + kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, + [this](const std::string & control_mode) + { return this->onControlModeChangeRequest(control_mode); }); robot_manager_node_->registerParameter<std::string>( - "position_controller_name", "", kuka_drivers_core::ParameterSetAccessRights {false, true, - false, false, true}, [this](const std::string & controller_name) { - return this->onControllerNameChangeRequest(controller_name, true); - }); + "position_controller_name", "", + kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, + [this](const std::string & controller_name) + { return this->onControllerNameChangeRequest(controller_name, true); }); robot_manager_node_->registerParameter<std::string>( - "torque_controller_name", "", kuka_drivers_core::ParameterSetAccessRights {false, true, - false, false, true}, [this](const std::string & controller_name) { - return this->onControllerNameChangeRequest(controller_name, false); - }); + "torque_controller_name", "", + kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, + [this](const std::string & controller_name) + { return this->onControllerNameChangeRequest(controller_name, false); }); robot_manager_node_->registerParameter<std::string>( - "command_mode", POSITION_COMMAND, kuka_drivers_core::ParameterSetAccessRights {false, true, - true, - false, true}, [this](const std::string & command_mode) { - return this->onCommandModeChangeRequest(command_mode); - }); + "command_mode", POSITION_COMMAND, + kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, + [this](const std::string & command_mode) + { return this->onCommandModeChangeRequest(command_mode); }); robot_manager_node_->registerParameter<int>( - "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights {false, true, false, - false, - true}, [this](const int & receive_multiplier) { - return this->onReceiveMultiplierChangeRequest(receive_multiplier); - }); + "receive_multiplier", 1, + kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, + [this](const int & receive_multiplier) + { return this->onReceiveMultiplierChangeRequest(receive_multiplier); }); configured_ = true; response->success = true; diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp index 7e611536..d1b38214 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include <chrono> #include <memory> -#include <vector> #include <thread> -#include <chrono> +#include <vector> -#include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "communication_helpers/serialization.hpp" +#include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/tcp_connection.hpp" namespace kuka_sunrise_fri_driver @@ -28,32 +28,35 @@ FRIConnection::FRIConnection( std::function<void(void)> handle_control_ended_error_callback, std::function<void(void)> handle_fri_ended_callback) : handleControlEndedError_(handle_control_ended_error_callback), - handleFRIEndedError_(handle_fri_ended_callback), last_command_state_(ACCEPTED), - last_command_id_(CONNECT), last_command_success_(NO_SUCCESS), answer_wanted_(false), + handleFRIEndedError_(handle_fri_ended_callback), + last_command_state_(ACCEPTED), + last_command_id_(CONNECT), + last_command_success_(NO_SUCCESS), + answer_wanted_(false), answer_received_(false) { } -FRIConnection::~FRIConnection() -{ - disconnect(); -} +FRIConnection::~FRIConnection() { disconnect(); } bool FRIConnection::connect(const char * server_addr, int server_port) { // TODO(resizoltan) check if already connected - try { + try + { tcp_connection_ = std::make_unique<TCPConnection>( - server_addr, - server_port, - [this](std::vector<std::uint8_t> data) {this->handleReceivedTCPData(data);}, - [this](const char * server_addr, - int server_port) {this->connectionLostCallback(server_addr, server_port);}); - } catch (...) { + server_addr, server_port, + [this](std::vector<std::uint8_t> data) { this->handleReceivedTCPData(data); }, + [this](const char * server_addr, int server_port) + { this->connectionLostCallback(server_addr, server_port); }); + } + catch (...) + { tcp_connection_.reset(); } - if (!isConnected()) { + if (!isConnected()) + { return false; } return sendCommandAndWait(CONNECT); @@ -61,35 +64,29 @@ bool FRIConnection::connect(const char * server_addr, int server_port) bool FRIConnection::disconnect() { - if (tcp_connection_ == nullptr) {return true;} - if (sendCommandAndWait(DISCONNECT) == true) { + if (tcp_connection_ == nullptr) + { + return true; + } + if (sendCommandAndWait(DISCONNECT) == true) + { tcp_connection_->closeConnection(); tcp_connection_.reset(); return true; - } else { + } + else + { return false; } } -bool FRIConnection::startFRI() -{ - return sendCommandAndWait(START_FRI); -} +bool FRIConnection::startFRI() { return sendCommandAndWait(START_FRI); } -bool FRIConnection::endFRI() -{ - return sendCommandAndWait(END_FRI); -} +bool FRIConnection::endFRI() { return sendCommandAndWait(END_FRI); } -bool FRIConnection::activateControl() -{ - return sendCommandAndWait(ACTIVATE_CONTROL); -} +bool FRIConnection::activateControl() { return sendCommandAndWait(ACTIVATE_CONTROL); } -bool FRIConnection::deactivateControl() -{ - return sendCommandAndWait(DEACTIVATE_CONTROL); -} +bool FRIConnection::deactivateControl() { return sendCommandAndWait(DEACTIVATE_CONTROL); } bool FRIConnection::setPositionControlMode() { @@ -98,8 +95,7 @@ bool FRIConnection::setPositionControlMode() } bool FRIConnection::setJointImpedanceControlMode( - const std::vector<double> & joint_stiffness, - const std::vector<double> & joint_damping) + const std::vector<double> & joint_stiffness, const std::vector<double> & joint_damping) { int msg_size = 0; printf("Sizeof(double) = %lu\n", sizeof(double)); @@ -110,15 +106,18 @@ bool FRIConnection::setJointImpedanceControlMode( serialized.reserve(1 + CONTROL_MODE_HEADER.size() + 2 * 7 * sizeof(double)); serialized.emplace_back(JOINT_IMPEDANCE_CONTROL_MODE); msg_size++; - for (std::uint8_t byte : CONTROL_MODE_HEADER) { + for (std::uint8_t byte : CONTROL_MODE_HEADER) + { serialized.emplace_back(byte); msg_size++; } - for (double js : joint_stiffness) { + for (double js : joint_stiffness) + { printf("js = %lf\n", js); msg_size += kuka_drivers_core::serializeNext(js, serialized); } - for (double jd : joint_damping) { + for (double jd : joint_damping) + { printf("jd = %lf\n", jd); msg_size += kuka_drivers_core::serializeNext(jd, serialized); } @@ -136,7 +135,8 @@ bool FRIConnection::setFRIConfig(int remote_port, int send_period_ms, int receiv std::vector<std::uint8_t> serialized; serialized.reserve(FRI_CONFIG_HEADER.size() + 3 * sizeof(int)); int msg_size = 0; - for (std::uint8_t byte : FRI_CONFIG_HEADER) { + for (std::uint8_t byte : FRI_CONFIG_HEADER) + { serialized.emplace_back(byte); msg_size++; } @@ -148,22 +148,27 @@ bool FRIConnection::setFRIConfig(int remote_port, int send_period_ms, int receiv bool FRIConnection::isConnected() { - if (tcp_connection_) { + if (tcp_connection_) + { return true; - } else { + } + else + { return false; } } - bool FRIConnection::assertLastCommandSuccess(CommandID command_id) { // TODO(resizoltan) more sophisticated introspection - if (last_command_state_ == ACCEPTED && last_command_id_ == command_id && + if ( + last_command_state_ == ACCEPTED && last_command_id_ == command_id && last_command_success_ == SUCCESS) { return true; - } else { + } + else + { return false; } } @@ -173,17 +178,14 @@ bool FRIConnection::sendCommandAndWait(CommandID command_id) answer_wanted_ = true; tcp_connection_->sendByte(command_id); std::unique_lock<std::mutex> lk(m_); - cv_.wait( - lk, [this] - {return answer_received_;}); + cv_.wait(lk, [this] { return answer_received_; }); answer_received_ = false; answer_wanted_ = false; return assertLastCommandSuccess(command_id); } bool FRIConnection::sendCommandAndWait( - CommandID command_id, - const std::vector<std::uint8_t> & command_data) + CommandID command_id, const std::vector<std::uint8_t> & command_data) { std::vector<std::uint8_t> msg; msg.push_back(command_id); @@ -191,9 +193,7 @@ bool FRIConnection::sendCommandAndWait( answer_wanted_ = true; tcp_connection_->sendBytes(msg); std::unique_lock<std::mutex> lk(m_); - cv_.wait( - lk, [this] - {return answer_received_;}); + cv_.wait(lk, [this] { return answer_received_; }); answer_received_ = false; answer_wanted_ = false; return assertLastCommandSuccess(command_id); @@ -201,15 +201,18 @@ bool FRIConnection::sendCommandAndWait( void FRIConnection::handleReceivedTCPData(const std::vector<std::uint8_t> & data) { - if (data.size() == 0) { + if (data.size() == 0) + { return; } - pthread_t handler_thread; + std::thread handler_thread; std::lock_guard<std::mutex> lk(m_); // TODO(resizoltan) handle invalid data - switch ((CommandState)data[0]) { + switch ((CommandState)data[0]) + { case ACCEPTED: - if (data.size() < 3) { + if (data.size() < 3) + { // TODO(resizoltan) error } last_command_state_ = ACCEPTED; @@ -219,7 +222,8 @@ void FRIConnection::handleReceivedTCPData(const std::vector<std::uint8_t> & data cv_.notify_one(); break; case REJECTED: - if (data.size() < 2) { + if (data.size() < 2) + { // TODO(resizoltan) error } last_command_state_ = REJECTED; @@ -233,37 +237,29 @@ void FRIConnection::handleReceivedTCPData(const std::vector<std::uint8_t> & data cv_.notify_one(); break; case ERROR_CONTROL_ENDED: - if (answer_wanted_) { + if (answer_wanted_) + { last_command_state_ = ERROR_CONTROL_ENDED; answer_received_ = true; cv_.notify_one(); - } else { - pthread_create( - &handler_thread, - nullptr, - [](void * self) -> void * { - static_cast<FRIConnection *>(self)->handleControlEndedError_(); - return nullptr; - }, - this); - pthread_detach(handler_thread); // TODO(resizoltan) ther might be a better way to do this + } + else + { + handler_thread = std::thread([this] { this->handleControlEndedError_(); }); + handler_thread.detach(); // Detach the thread } break; case ERROR_FRI_ENDED: - if (answer_wanted_) { + if (answer_wanted_) + { last_command_state_ = ERROR_FRI_ENDED; answer_received_ = true; cv_.notify_one(); - } else { - pthread_create( - &handler_thread, - nullptr, - [](void * self) -> void * { - static_cast<FRIConnection *>(self)->handleFRIEndedError_(); - return nullptr; - }, - this); - pthread_detach(handler_thread); // TODO(resizoltan) ther might be a better way to do this + } + else + { + handler_thread = std::thread([this] { this->handleFRIEndedError_(); }); + handler_thread.detach(); // Detach the thread } break; default: diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/tcp_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/tcp_connection.cpp index 905e7927..f1c566eb 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/tcp_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/tcp_connection.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include <stdexcept> -#include <vector> #include <string> +#include <vector> #include "kuka_sunrise_fri_driver/tcp_connection.hpp" @@ -25,21 +25,26 @@ TCPConnection::TCPConnection( const char * server_addr, int server_port, std::function<void(const std::vector<std::uint8_t> &)> data_received_callback, std::function<void(const char * server_addr, const int server_port)> connection_lost_callback) -: dataReceivedCallback_(data_received_callback), connectionLostCallback_( - connection_lost_callback), socket_desc_(socket(AF_INET, SOCK_STREAM, 0)), cancelled_(false) +: dataReceivedCallback_(data_received_callback), + connectionLostCallback_(connection_lost_callback), + socket_desc_(socket(AF_INET, SOCK_STREAM, 0)), + cancelled_(false) { - if (socket_desc_ == -1) { + if (socket_desc_ == -1) + { throw std::runtime_error("Could not create socket"); } - if (inet_aton(server_addr, &server_.sin_addr) == 0) { + if (inet_aton(server_addr, &server_.sin_addr) == 0) + { close(socket_desc_); - std::string errormsg = std::string("Received invalid server IP address: ") + - std::string(server_addr); + std::string errormsg = + std::string("Received invalid server IP address: ") + std::string(server_addr); throw std::invalid_argument(errormsg.c_str()); } server_.sin_family = AF_INET; server_.sin_port = htons(server_port); - if (connect(socket_desc_, (struct sockaddr *)&server_, sizeof(server_))) { + if (connect(socket_desc_, (struct sockaddr *)&server_, sizeof(server_))) + { throw std::runtime_error("Could not connect to server"); } pthread_create(&read_thread_, NULL, &TCPConnection::listen_helper, this); @@ -48,7 +53,8 @@ TCPConnection::TCPConnection( bool TCPConnection::sendByte(std::uint8_t data) { int sent_length = write(socket_desc_, &data, 1); - if (sent_length < 0) { + if (sent_length < 0) + { return false; } // TODO(resizoltan) handle other kind of errors? return true; @@ -57,7 +63,8 @@ bool TCPConnection::sendByte(std::uint8_t data) bool TCPConnection::sendBytes(const std::vector<std::uint8_t> & data) { int sent_length = write(socket_desc_, data.data(), data.size()); - if (sent_length < 0) { + if (sent_length < 0) + { return false; } // TODO(resizoltan) handle other kind of errors? return true; @@ -88,17 +95,24 @@ void * TCPConnection::listen_helper(void * tcpConnection) void TCPConnection::listen() { std::uint8_t msg_buffer[300]; - while (!cancelled_.load()) { + while (!cancelled_.load()) + { int length = recv(socket_desc_, msg_buffer, 300, 0); - if (length < 0) { - if (cancelled_.load()) { + if (length < 0) + { + if (cancelled_.load()) + { break; } // TODO(resizoltan) handle error - } else if (length == 0) { // TODO(resizoltan) is this the way to check for connection loss? + } + else if (length == 0) + { // TODO(resizoltan) is this the way to check for connection loss? connectionLostCallback_(inet_ntoa(server_.sin_addr), ntohs(server_.sin_port)); break; - } else { + } + else + { std::vector<std::uint8_t> server_msg(msg_buffer, msg_buffer + length); dataReceivedCallback_(server_msg); } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c index e6700e55..711f8cfa 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/FRIMessages.pb.c @@ -69,4 +69,3 @@ PB_BIND(FRICommandMessage, FRICommandMessage, 2) */ PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) #endif - diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp index ae900eb1..249ab8d3 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/HWIFClientApplication.cpp @@ -48,7 +48,7 @@ void HWIFClientApplication::client_app_update() // ************************************************************************** // callbacks // ************************************************************************** - // reset commmand message before callbacks + // reset command message before callbacks _data->resetCommandMessage(); // callbacks for robot client diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp index faa9235b..d94eec77 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friClientApplication.cpp @@ -141,7 +141,7 @@ bool ClientApplication::step() // ************************************************************************** // callbacks // ************************************************************************** - // reset commmand message before callbacks + // reset command message before callbacks _data->resetCommandMessage(); // callbacks for robot client diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp index 4bada7f4..e222e273 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/friUdpConnection.cpp @@ -189,7 +189,7 @@ int UdpConnection::receive(char * buffer, int maxSize) // initialize file descriptor /** * Replace FD_ZERO with memset, because bzero is not available for VxWorks - * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + * User Space Applications(RTPs). Therefore the macro FD_ZERO does not compile. */ #ifndef VXWORKS FD_ZERO(&_filedescriptor); @@ -207,7 +207,7 @@ int UdpConnection::receive(char * buffer, int maxSize) } // a negative value indicates an error else if (numberActiveFileDescr == -1) { - printf("An error has occured \n"); + printf("An error has occurred \n"); return -1; } } diff --git a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c index 244e588f..4a1407a1 100644 --- a/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c +++ b/kuka_sunrise_fri_driver/src/fri_client_sdk/pb_frimessages_callbacks.c @@ -14,16 +14,16 @@ code, libraries, binaries, manuals and technical documentation. COPYRIGHT All Rights Reserved -Copyright (C) 2014-2019 +Copyright (C) 2014-2019 KUKA Roboter GmbH Augsburg, Germany -LICENSE +LICENSE Redistribution and use of the software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: -a) The software is used in conjunction with KUKA products only. +a) The software is used in conjunction with KUKA products only. b) Redistributions of source code must retain the above copyright notice, this list of conditions and the disclaimer. c) Redistributions in binary form must reproduce the above copyright notice, @@ -40,14 +40,14 @@ DISCLAIMER OF WARRANTY The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of any kind, including without limitation the warranties of merchantability, -fitness for a particular purpose and non-infringement. +fitness for a particular purpose and non-infringement. KUKA makes no warranty that the Software is free of defects or is suitable for any particular purpose. In no event shall KUKA be responsible for loss or damages arising from the installation or use of the Software, including but not limited to any indirect, punitive, special, incidental or consequential damages of any character including, without limitation, damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other -commercial damages or losses. +commercial damages or losses. The entire risk to the quality and performance of the Software is not borne by KUKA. Should the Software prove defective, KUKA is not liable for the entire cost of any service and repair. @@ -59,7 +59,7 @@ cost of any service and repair. \version {1.15} */ #include <stdio.h> -#include <stdlib.h> +#include <stdlib.h> #include <pb_frimessages_callbacks.h> #include <pb_encode.h> @@ -194,7 +194,7 @@ bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **ar void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { @@ -217,7 +217,7 @@ void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) { - // IMPORTANT: the callbacks are stored in a union, therefor a message object + // IMPORTANT: the callbacks are stored in a union, therefore a message object // must be exclusive defined for transmission or reception if (dir == FRI_MANAGER_NANOPB_ENCODE) { diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 0845f35b..443136ed 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -24,7 +24,8 @@ namespace kuka_sunrise_fri_driver CallbackReturn KukaFRIHardwareInterface::on_init( const hardware_interface::HardwareInfo & system_info) { - if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } hw_states_.resize(info_.joints.size()); @@ -33,15 +34,13 @@ CallbackReturn KukaFRIHardwareInterface::on_init( hw_torques_ext_.resize(info_.joints.size()); hw_effort_command_.resize(info_.joints.size()); - if (info_.gpios.size() != 1) { - RCLCPP_FATAL( - rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting exactly 1 GPIO"); + if (info_.gpios.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 1 GPIO"); return CallbackReturn::ERROR; } - if (info_.gpios[0].command_interfaces.size() > 10 || - info_.gpios[0].state_interfaces.size() > 10) + if (info_.gpios[0].command_interfaces.size() > 10 || info_.gpios[0].state_interfaces.size() > 10) { RCLCPP_FATAL( rclcpp::get_logger("KukaFRIHardwareInterface"), @@ -49,82 +48,82 @@ CallbackReturn KukaFRIHardwareInterface::on_init( return CallbackReturn::ERROR; } - for (const auto & state_if : info_.gpios[0].state_interfaces) { + for (const auto & state_if : info_.gpios[0].state_interfaces) + { gpio_outputs_.emplace_back(state_if.name, getType(state_if.data_type), robotState()); } - for (const auto & command_if : info_.gpios[0].command_interfaces) { + for (const auto & command_if : info_.gpios[0].command_interfaces) + { gpio_inputs_.emplace_back( - command_if.name, getType(command_if.data_type), - robotCommand(), std::stod(command_if.initial_value)); + command_if.name, getType(command_if.data_type), robotCommand(), + std::stod(command_if.initial_value)); } - - for (const hardware_interface::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + if (joint.command_interfaces.size() != 2) + { RCLCPP_FATAL( - rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting exactly 2 command interface"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 2 command interface"); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting POSITION command interface as first"); + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting POSITION command interface as first"); return CallbackReturn::ERROR; } - if (joint.command_interfaces[1].name != hardware_interface::HW_IF_EFFORT) { + if (joint.command_interfaces[1].name != hardware_interface::HW_IF_EFFORT) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting EFFORT command interface as second"); + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting EFFORT command interface as second"); return CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 3) { + if (joint.state_interfaces.size() != 3) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting exactly 3 state interface"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 3 state interface"); return CallbackReturn::ERROR; } - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting POSITION state interface as first"); + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting POSITION state interface as first"); return CallbackReturn::ERROR; } - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_EFFORT) { + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_EFFORT) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting EFFORT state interface as second"); + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting EFFORT state interface as second"); return CallbackReturn::ERROR; } - if (joint.state_interfaces[2].name != "external_torque") { + if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EXTERNAL_TORQUE) + { RCLCPP_FATAL( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "expecting 'external torque' state interface as third"); + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting 'EXTERNAL_TORQUE' state interface as third"); return CallbackReturn::ERROR; } } - struct sched_param param; - param.sched_priority = 95; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "setscheduler error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), strerror(errno)); - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; } CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - if (!client_application_.connect(30200, nullptr)) { + if (!client_application_.connect(30200, nullptr)) + { RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not connect"); return CallbackReturn::FAILURE; } @@ -145,7 +144,8 @@ void KukaFRIHardwareInterface::waitForCommand() hw_effort_command_ = hw_torques_; // TODO(Svastits): is this really the purpose of waitForCommand? rclcpp::Time stamp = ros_clock_.now(); - if (++receive_counter_ == receive_multiplier_) { + if (++receive_counter_ == receive_multiplier_) + { updateCommand(stamp); receive_counter_ = 0; } @@ -154,19 +154,19 @@ void KukaFRIHardwareInterface::waitForCommand() void KukaFRIHardwareInterface::command() { rclcpp::Time stamp = ros_clock_.now(); - if (++receive_counter_ == receive_multiplier_) { + if (++receive_counter_ == receive_multiplier_) + { updateCommand(stamp); receive_counter_ = 0; } } - hardware_interface::return_type KukaFRIHardwareInterface::read( - const rclcpp::Time &, - const rclcpp::Duration &) + const rclcpp::Time &, const rclcpp::Duration &) { // Read is called in inactive state, check is necessary - if (!is_active_) { + if (!is_active_) + { active_read_ = false; RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -174,10 +174,10 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( } active_read_ = true; - if (!client_application_.client_app_read()) { + if (!client_application_.client_app_read()) + { RCLCPP_ERROR( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "Failed to read data from controller"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "Failed to read data from controller"); return hardware_interface::return_type::ERROR; } @@ -199,7 +199,8 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( robot_state_.drive_state_ = robotState().getDriveState(); robot_state_.overlay_type_ = robotState().getOverlayType(); - for (auto & output : gpio_outputs_) { + for (auto & output : gpio_outputs_) + { output.getValue(); } @@ -207,11 +208,11 @@ hardware_interface::return_type KukaFRIHardwareInterface::read( } hardware_interface::return_type KukaFRIHardwareInterface::write( - const rclcpp::Time &, - const rclcpp::Duration &) + const rclcpp::Time &, const rclcpp::Duration &) { // Client app update and read must be called if read has been called in current cycle - if (!active_read_) { + if (!active_read_) + { RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); return hardware_interface::return_type::OK; } @@ -220,10 +221,10 @@ hardware_interface::return_type KukaFRIHardwareInterface::write( // this updates the command to be sent based on the output of the controller update client_application_.client_app_update(); - if (!client_application_.client_app_write() && is_active_) { + if (!client_application_.client_app_write() && is_active_) + { RCLCPP_ERROR( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "Could not send command to controller"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not send command to controller"); return hardware_interface::return_type::ERROR; } @@ -232,23 +233,29 @@ hardware_interface::return_type KukaFRIHardwareInterface::write( void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { - if (!is_active_) { + if (!is_active_) + { RCLCPP_ERROR( - rclcpp::get_logger( - "KukaFRIHardwareInterface"), "Hardware inactive, exiting updateCommand"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware inactive, exiting updateCommand"); return; } - if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) { + if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) + { const double * joint_torques_ = hw_effort_command_.data(); robotCommand().setJointPosition(robotState().getIpoJointPosition()); robotCommand().setTorque(joint_torques_); - } else if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) { + } + else if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) + { const double * joint_positions_ = hw_commands_.data(); robotCommand().setJointPosition(joint_positions_); - } else { + } + else + { RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Unsupported command mode"); } - for (auto & input : gpio_inputs_) { + for (auto & input : gpio_inputs_) + { input.setValue(); } } @@ -286,57 +293,52 @@ std::vector<hardware_interface::StateInterface> KukaFRIHardwareInterface::export &robot_state_.tracking_performance_); // Register I/O outputs (read access) - for (auto & output : gpio_outputs_) { + for (auto & output : gpio_outputs_) + { state_interfaces.emplace_back( - hardware_interface::IO_PREFIX, output.getName(), - &output.getData()); + hardware_interface::IO_PREFIX, output.getName(), &output.getData()); } - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_states_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]); state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_torques_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torques_[i]); state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, - &hw_torques_ext_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_torques_ext_[i]); } return state_interfaces; } -std::vector<hardware_interface::CommandInterface> KukaFRIHardwareInterface:: -export_command_interfaces() +std::vector<hardware_interface::CommandInterface> +KukaFRIHardwareInterface::export_command_interfaces() { std::vector<hardware_interface::CommandInterface> command_interfaces; command_interfaces.emplace_back( - hardware_interface::CONFIG_PREFIX, - hardware_interface::RECEIVE_MULTIPLIER, &receive_multiplier_); + hardware_interface::CONFIG_PREFIX, hardware_interface::RECEIVE_MULTIPLIER, + &receive_multiplier_); // Register I/O inputs (write access) - for (auto & input : gpio_inputs_) { + for (auto & input : gpio_inputs_) + { command_interfaces.emplace_back( - hardware_interface::IO_PREFIX, input.getName(), - &input.getData()); + hardware_interface::IO_PREFIX, input.getName(), &input.getData()); } - for (size_t i = 0; i < info_.joints.size(); i++) { + for (size_t i = 0; i < info_.joints.size(); i++) + { command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_effort_command_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_effort_command_[i]); } return command_interfaces; } } // namespace kuka_sunrise_fri_driver PLUGINLIB_EXPORT_CLASS( - kuka_sunrise_fri_driver::KukaFRIHardwareInterface, - hardware_interface::SystemInterface -) + kuka_sunrise_fri_driver::KukaFRIHardwareInterface, hardware_interface::SystemInterface) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index eed83785..56b6a529 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -14,8 +14,8 @@ #include <memory> -#include "communication_helpers/service_tools.hpp" #include "communication_helpers/ros2_control_tools.hpp" +#include "communication_helpers/service_tools.hpp" #include "kuka_sunrise_fri_driver/robot_manager_node.hpp" @@ -23,8 +23,7 @@ using namespace controller_manager_msgs::srv; // NOLINT namespace kuka_sunrise_fri_driver { -RobotManagerNode::RobotManagerNode() -: kuka_drivers_core::ROS2BaseLCNode("robot_manager") +RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager") { // Controllers do not support the cleanup transition (as of now) // Therefore controllers are loaded and configured at startup, only activation @@ -37,36 +36,30 @@ RobotManagerNode::RobotManagerNode() // non-RT controllers are started after interface configuration fri_connection_ = std::make_shared<FRIConnection>( - [this] - {this->handleControlEndedError();}, [this] - {this->handleFRIEndedError();}); + [this] { this->handleControlEndedError(); }, [this] { this->handleFRIEndedError(); }); auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - change_hardware_state_client_ = - this->create_client<SetHardwareComponentState>( + change_hardware_state_client_ = this->create_client<SetHardwareComponentState>( "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); - change_controller_state_client_ = - this->create_client<SwitchController>( + change_controller_state_client_ = this->create_client<SwitchController>( "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_); - command_state_changed_publisher_ = this->create_publisher<std_msgs::msg::Bool>( - "robot_manager/commanding_state_changed", qos); + command_state_changed_publisher_ = + this->create_publisher<std_msgs::msg::Bool>("robot_manager/commanding_state_changed", qos); set_parameter_client_ = this->create_client<std_srvs::srv::Trigger>( "configuration_manager/set_params", ::rmw_qos_profile_default, cbg_); auto is_configured_qos = rclcpp::QoS(rclcpp::KeepLast(1)); is_configured_qos.best_effort(); - is_configured_pub_ = this->create_publisher<std_msgs::msg::Bool>( - "robot_manager/is_configured", - is_configured_qos); + is_configured_pub_ = + this->create_publisher<std_msgs::msg::Bool>("robot_manager/is_configured", is_configured_qos); this->registerStaticParameter<std::string>( "robot_model", "lbr_iiwa14_r820", - kuka_drivers_core::ParameterSetAccessRights{true, false, - false, false, false}, [this](const std::string & robot_model) { - return this->onRobotModelChangeRequest(robot_model); - }); + kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + [this](const std::string & robot_model) + { return this->onRobotModelChangeRequest(robot_model); }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -74,8 +67,8 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) + change_hardware_state_client_, robot_model_, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); return FAILURE; @@ -85,48 +78,57 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // If this fails, the node should be restarted, with different parameter values // Therefore exceptions are not caught - if (!configuration_manager_) { + if (!configuration_manager_) + { configuration_manager_ = std::make_unique<ConfigurationManager>( - std::dynamic_pointer_cast<kuka_drivers_core::ROS2BaseLCNode>( - this->shared_from_this()), fri_connection_); + std::dynamic_pointer_cast<kuka_drivers_core::ROS2BaseLCNode>(this->shared_from_this()), + fri_connection_); } RCLCPP_INFO(get_logger(), "Successfully set 'controller_ip' parameter"); // Start non-RT controllers if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"fri_configuration_controller", "fri_state_broadcaster"}, - {})) + change_controller_state_client_, {"fri_configuration_controller", "fri_state_broadcaster"}, + {})) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); result = FAILURE; } const char * controller_ip = this->get_parameter("controller_ip").as_string().c_str(); - if (!fri_connection_->isConnected()) { - if (!fri_connection_->connect(controller_ip, 30000)) { + if (!fri_connection_->isConnected()) + { + if (!fri_connection_->connect(controller_ip, 30000)) + { RCLCPP_ERROR(get_logger(), "could not connect"); result = FAILURE; } - } else { + } + else + { RCLCPP_ERROR(get_logger(), "Robot manager is connected in inactive state"); return ERROR; } RCLCPP_INFO(get_logger(), "Successfully connected to FRI"); - if (result == SUCCESS) { - auto trigger_request = - std::make_shared<std_srvs::srv::Trigger::Request>(); + if (result == SUCCESS) + { + auto trigger_request = std::make_shared<std_srvs::srv::Trigger::Request>(); auto response = kuka_drivers_core::sendRequest<std_srvs::srv::Trigger::Response>( set_parameter_client_, trigger_request, 0, 1000); - if (!response || !response->success) { + if (!response || !response->success) + { RCLCPP_ERROR(get_logger(), "Could not set parameters"); result = FAILURE; } } - if (result != SUCCESS) { + if (result != SUCCESS) + { this->on_cleanup(get_current_state()); - } else { + } + else + { is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); @@ -138,7 +140,8 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { - if (fri_connection_->isConnected() && !fri_connection_->disconnect()) { + if (fri_connection_->isConnected() && !fri_connection_->disconnect()) + { RCLCPP_ERROR(get_logger(), "could not disconnect"); return ERROR; } @@ -146,9 +149,9 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // Stop non-RT controllers // With best effort strictness, cleanup succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - {"fri_configuration_controller", "fri_state_broadcaster"}, - SwitchController::Request::BEST_EFFORT)) + change_controller_state_client_, {}, + {"fri_configuration_controller", "fri_state_broadcaster"}, + SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); return ERROR; @@ -157,14 +160,15 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // Cleanup hardware interface // If it is inactive, cleanup will also succeed if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)) + change_hardware_state_client_, robot_model_, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)) { RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); return FAILURE; } - if (is_configured_pub_->is_activated()) { + if (is_configured_pub_->is_activated()) + { is_configured_msg_.data = false; is_configured_pub_->publish(is_configured_msg_); is_configured_pub_->on_deactivate(); @@ -173,20 +177,21 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) return SUCCESS; } - // TODO(Svastits): can we check if necessary 5s has passed after deactivation? // TODO(Svastits): check if we have to send unconfigured msg to control node rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { - if (!fri_connection_->isConnected()) { + if (!fri_connection_->isConnected()) + { RCLCPP_ERROR(get_logger(), "not connected"); return ERROR; } auto send_period_ms = static_cast<int>(this->get_parameter("send_period_ms").as_int()); auto receive_multiplier = static_cast<int>(this->get_parameter("receive_multiplier").as_int()); - if (!fri_connection_->setFRIConfig(30200, send_period_ms, receive_multiplier)) { + if (!fri_connection_->setFRIConfig(30200, send_period_ms, receive_multiplier)) + { RCLCPP_ERROR(get_logger(), "could not set FRI config"); return FAILURE; } @@ -194,8 +199,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) + change_hardware_state_client_, robot_model_, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); // 'unset config' does not exist, safe to return @@ -203,7 +208,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) } // Start FRI (in monitoring mode) - if (!fri_connection_->startFRI()) { + if (!fri_connection_->startFRI()) + { RCLCPP_ERROR(get_logger(), "Could not start FRI"); this->on_deactivate(get_current_state()); return FAILURE; @@ -214,8 +220,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // The other controller must be started later so that it can initialize internal state // with broadcaster information -> TODO(Svastits): validate whether this is true if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"joint_state_broadcaster"}, - {})) + change_controller_state_client_, {"joint_state_broadcaster"}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate joint state broadcaster"); this->on_deactivate(get_current_state()); @@ -224,12 +229,12 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) auto position_controller_name = this->get_parameter("position_controller_name").as_string(); auto torque_controller_name = this->get_parameter("torque_controller_name").as_string(); - controller_name_ = (this->get_parameter("command_mode").as_string() == - "position") ? position_controller_name : torque_controller_name; + controller_name_ = (this->get_parameter("command_mode").as_string() == "position") + ? position_controller_name + : torque_controller_name; // Activate RT commander if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {controller_name_}, - {})) + change_controller_state_client_, {controller_name_}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controller"); this->on_deactivate(get_current_state()); @@ -238,7 +243,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) command_state_changed_publisher_->on_activate(); // Start commanding mode - if (!activateControl()) { + if (!activateControl()) + { this->on_deactivate(get_current_state()); return FAILURE; } @@ -249,17 +255,20 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) { - if (!fri_connection_->isConnected()) { + if (!fri_connection_->isConnected()) + { RCLCPP_ERROR(get_logger(), "Not connected"); return ERROR; } - if (!this->deactivateControl()) { + if (!this->deactivateControl()) + { RCLCPP_ERROR(get_logger(), "Could not deactivate control"); return ERROR; } - if (!fri_connection_->endFRI()) { + if (!fri_connection_->endFRI()) + { RCLCPP_ERROR(get_logger(), "Could not end FRI"); return ERROR; } @@ -267,8 +276,8 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Deactivate hardware interface // If it is inactive, deactivation will also succeed if (!kuka_drivers_core::changeHardwareState( - change_hardware_state_client_, robot_model_, - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) + change_hardware_state_client_, robot_model_, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; @@ -277,14 +286,15 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - {controller_name_, "joint_state_broadcaster"}, SwitchController::Request::BEST_EFFORT)) + change_controller_state_client_, {}, {controller_name_, "joint_state_broadcaster"}, + SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not deactivate controllers"); return ERROR; } - if (command_state_changed_publisher_->is_activated()) { + if (command_state_changed_publisher_->is_activated()) + { command_state_changed_publisher_->on_deactivate(); } @@ -295,12 +305,14 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) bool RobotManagerNode::activateControl() { - if (!fri_connection_->isConnected()) { + if (!fri_connection_->isConnected()) + { RCLCPP_ERROR(get_logger(), "Not connected"); return false; } - if (!fri_connection_->activateControl()) { + if (!fri_connection_->activateControl()) + { RCLCPP_ERROR(get_logger(), "Could not activate control"); return false; } @@ -312,12 +324,14 @@ bool RobotManagerNode::activateControl() bool RobotManagerNode::deactivateControl() { - if (!fri_connection_->isConnected()) { + if (!fri_connection_->isConnected()) + { RCLCPP_ERROR(get_logger(), "Not connected"); return false; } - if (!fri_connection_->deactivateControl()) { + if (!fri_connection_->deactivateControl()) + { RCLCPP_ERROR(get_logger(), "Could not deactivate control"); return false; } diff --git a/kuka_sunrise_fri_driver/test/test_driver_startup.py b/kuka_sunrise_fri_driver/test/test_driver_startup.py new file mode 100644 index 00000000..19e56e27 --- /dev/null +++ b/kuka_sunrise_fri_driver/test/test_driver_startup.py @@ -0,0 +1,58 @@ +# Copyright 2024 Áron Svastits +# +# 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 unittest + +import launch +import launch.actions +import launch_testing.actions +import launch_testing.markers +import pytest + +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) # noqa: E501 +from launch.actions.include_launch_description import IncludeLaunchDescription +from ament_index_python.packages import get_package_share_directory + + +# Launch all of the robot visualisation launch files one by one +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + return launch.LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + get_package_share_directory("kuka_sunrise_fri_driver"), + "/launch/", + "startup.launch.py", + ] + ) + ), + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestModels(unittest.TestCase): + def test_read_stdout(self, proc_output): + # Check for successful initialization + proc_output.assertWaitFor("got segment base", timeout=5) + proc_output.assertWaitFor( + "Successful initialization of hardware 'lbr_iiwa14_r820'", timeout=5 + ) + # Check whether disabling automatic activation was successful + proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) diff --git a/upstream.repos b/upstream.repos index 845df97b..7aec6253 100644 --- a/upstream.repos +++ b/upstream.repos @@ -7,4 +7,4 @@ repositories: kuka_controllers: type: git url: https://github.com/kroshu/kuka_controllers.git - version: master \ No newline at end of file + version: master