├── .clang-format ├── .codespell-ignore-words.txt ├── .github └── workflows │ └── ci-format.yml ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── docker ├── Dockerfile └── docker-compose.yaml ├── hercules_description ├── CMakeLists.txt ├── config │ ├── dual_ur_controllers.yaml │ ├── hercules_controllers.yaml │ ├── initial_positions_port_arm.yaml │ ├── initial_positions_starboard_arm.yaml │ └── test_dual_goal_publishers_config.yaml ├── launch │ ├── dual_ur_sim_control.launch.py │ ├── hercules_controllers.launch.py │ ├── hercules_sim_control.launch.py │ ├── test_dual_ur_controllers.launch.py │ └── test_hercules_controllers.launch.py ├── package.xml ├── rviz │ └── view_robot.rviz └── urdf │ ├── dual_ur.urdf.xacro │ ├── hercules.urdf.xacro │ └── hercules_macro.urdf.xacro ├── roscon2023_control_workshop.ci.repos ├── roscon2023_control_workshop.repos ├── roscon2023_control_workshop ├── CMakeLists.txt ├── LICENSE └── package.xml ├── tiago_chaining ├── CMakeLists.txt ├── config │ ├── controllers.yaml │ ├── model_and_tf.rviz │ └── tiago_full_unrolled.urdf ├── launch │ ├── key_teleop.launch.xml │ ├── rviz.launch.xml │ ├── tiago_chaining.launch.xml │ ├── tiago_show.launch.xml │ └── tiago_warmup.launch.xml └── package.xml └── twist_relay ├── CMakeLists.txt ├── controller_plugins.xml ├── include └── twist_relay │ └── twist_relay_controller.hpp ├── package.xml └── src ├── twist_relay_controller.cpp └── twist_relay_controller_parameter.yaml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | ColumnLimit: 100 6 | AccessModifierOffset: -2 7 | AlignAfterOpenBracket: AlwaysBreak 8 | BreakBeforeBraces: Allman 9 | ConstructorInitializerIndentWidth: 0 10 | ContinuationIndentWidth: 2 11 | DerivePointerAlignment: false 12 | PointerAlignment: Middle 13 | ReflowComments: true 14 | IncludeBlocks: Preserve 15 | ... 16 | -------------------------------------------------------------------------------- /.codespell-ignore-words.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roscon2023_control_workshop/cc3e4d0ec0c9deba87f23ebdb2bde7f503348859/.codespell-ignore-words.txt -------------------------------------------------------------------------------- /.github/workflows/ci-format.yml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | 10 | jobs: 11 | pre-commit: 12 | name: Format 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v3 16 | - uses: actions/setup-python@v4.4.0 17 | with: 18 | python-version: '3.10' 19 | - name: Install system hooks 20 | run: sudo apt install -qq clang-format-14 cppcheck 21 | - uses: pre-commit/action@v3.0.0 22 | with: 23 | extra_args: --all-files --hook-stage manual 24 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | # files that should be ignored globally 16 | exclude: ^(docs/_themes/)|^(docs/_templates/) 17 | 18 | 19 | repos: 20 | 21 | # Standard hooks 22 | - repo: https://github.com/pre-commit/pre-commit-hooks 23 | rev: v4.4.0 24 | hooks: 25 | - id: check-added-large-files 26 | - id: check-ast 27 | - id: check-case-conflict 28 | - id: check-docstring-first 29 | - id: check-merge-conflict 30 | - id: check-symlinks 31 | - id: check-xml 32 | - id: check-yaml 33 | - id: debug-statements 34 | - id: destroyed-symlinks 35 | - id: detect-private-key 36 | - id: end-of-file-fixer 37 | - id: forbid-submodules 38 | - id: mixed-line-ending 39 | - id: requirements-txt-fixer 40 | - id: trailing-whitespace 41 | exclude_types: [rst] 42 | - id: fix-byte-order-marker 43 | 44 | 45 | # Python hooks 46 | - repo: https://github.com/asottile/pyupgrade 47 | rev: v3.3.2 48 | hooks: 49 | - id: pyupgrade 50 | args: [--py36-plus] 51 | 52 | - repo: https://github.com/PyCQA/pydocstyle 53 | rev: 6.3.0 54 | hooks: 55 | - id: pydocstyle 56 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 57 | 58 | - repo: https://github.com/pycqa/flake8 59 | rev: 6.0.0 60 | hooks: 61 | - id: flake8 62 | args: ["--extend-ignore=E501"] 63 | 64 | - repo: https://github.com/psf/black 65 | rev: 23.3.0 66 | hooks: 67 | - id: black 68 | args: ["--line-length=99"] 69 | 70 | 71 | # CPP hooks 72 | - repo: https://github.com/pre-commit/mirrors-clang-format 73 | rev: v14.0.6 74 | hooks: 75 | - id: clang-format 76 | 77 | - repo: local 78 | hooks: 79 | - id: ament_cppcheck 80 | name: ament_cppcheck 81 | description: Static code analysis of C/C++ files. 82 | stages: [commit] 83 | entry: ament_cppcheck 84 | language: system 85 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 86 | 87 | # Maybe use https://github.com/cpplint/cpplint instead 88 | - repo: local 89 | hooks: 90 | - id: ament_cpplint 91 | name: ament_cpplint 92 | description: Static code analysis of C/C++ files. 93 | stages: [commit] 94 | entry: ament_cpplint 95 | language: system 96 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 97 | args: ["--linelength=100", "--filter=-whitespace/newline"] 98 | 99 | # Cmake hooks 100 | - repo: local 101 | hooks: 102 | - id: ament_lint_cmake 103 | name: ament_lint_cmake 104 | description: Check format of CMakeLists.txt files. 105 | stages: [commit] 106 | entry: ament_lint_cmake 107 | language: system 108 | files: CMakeLists\.txt$ 109 | 110 | # Copyright 111 | - repo: local 112 | hooks: 113 | - id: ament_copyright 114 | name: ament_copyright 115 | description: Check if copyright notice is available in all files. 116 | stages: [commit] 117 | entry: ament_copyright 118 | language: system 119 | 120 | # Docs - RestructuredText hooks 121 | - repo: https://github.com/PyCQA/doc8 122 | rev: v1.1.1 123 | hooks: 124 | - id: doc8 125 | args: ['--max-line-length=100', '--ignore=D001'] 126 | exclude: CHANGELOG\.rst$ 127 | 128 | - repo: https://github.com/pre-commit/pygrep-hooks 129 | rev: v1.10.0 130 | hooks: 131 | - id: rst-backticks 132 | exclude: CHANGELOG\.rst$ 133 | - id: rst-directive-colons 134 | - id: rst-inline-touching-normal 135 | 136 | # Spellcheck in comments and docs 137 | # skipping of *.svg files is not working... 138 | - repo: https://github.com/codespell-project/codespell 139 | rev: v2.2.4 140 | hooks: 141 | - id: codespell 142 | args: ['--ignore-words=.codespell-ignore-words.txt', '--write-changes'] 143 | exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ 144 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | roscon2023_control_workshop ![Level](https://img.shields.io/badge/Level-Intermediate-red.svg) 2 | ================================================================================================ 3 | 4 | Support files for the ros2_control workshop at ROSCon 2023 in New Orleans, Louisiana, USA. 5 | 6 | ![Licence](https://img.shields.io/badge/License-Apache_2.0-blue.svg) 7 | 8 | 9 | # Installation 10 | 11 | Create a new workspace for ROS 2 rolling and clone this repository. 12 | Then add all the dependencies from source using `vcs` command: 13 | ``` 14 | 15 | vcs import --input roscon2023_control_workshop/roscon2023_control_workshop.ci.repos . 16 | ```` 17 | 18 | Then compile your workspace. In this case is is recommended to use `--symlink-install` flag for faster testing of changes. 19 | 20 | 21 | # Workshop Tasks and Examples 22 | 23 | ## x.Complex Hardware - Hercules 24 | 25 | 1. Try starting part of the Hercules with two URe5 arms and grippers: 26 | ``` 27 | ros2 launch hercules_description dual_ur_sim_control.launch.py 28 | ``` 29 | And execute example movements in another terminal using: 30 | ``` 31 | ros2 launch hercules_description test_dual_ur_controllers.launch.py 32 | ``` 33 | 34 | 1. Start the full Hercules setup and test movements executing the following command in separate terminals: 35 | ``` 36 | ros2 launch hercules_description hercules_sim_control.launch.py 37 | ``` 38 | ``` 39 | ros2 launch hercules_description test_hercules_controllers.launch.py 40 | ``` 41 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO="rolling" 2 | FROM ros:${ROS_DISTRO} 3 | 4 | RUN apt-get update && apt-get install -y git tig vim build-essential && \ 5 | rm -rf /var/lib/apt/lists/* 6 | 7 | # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size 8 | RUN apt-get update && \ 9 | apt-get install -y ros-${ROS_DISTRO}-ros2-control ros-${ROS_DISTRO}-ros2-controllers ros-${ROS_DISTRO}-kinematics-interface-kdl ros-${ROS_DISTRO}-rviz2 tmux terminator && \ 10 | rm -rf /var/lib/apt/lists/* 11 | 12 | # tiago-related deps 13 | RUN apt-get update && \ 14 | apt-get install -y ros-${ROS_DISTRO}-launch-param-builder ros-${ROS_DISTRO}-joint-state-publisher-gui && \ 15 | rm -rf /var/lib/apt/lists/* 16 | 17 | ENV ROS_LOCALHOST_ONLY=TRUE 18 | 19 | RUN mkdir -p /home/ros/ws/src 20 | 21 | RUN echo "alias s='source install/setup.bash'" >> /root/.bashrc 22 | RUN echo "alias cb='colcon build --symlink-install'" >> /root/.bashrc -------------------------------------------------------------------------------- /docker/docker-compose.yaml: -------------------------------------------------------------------------------- 1 | version: "3.9" 2 | 3 | services: 4 | dev: 5 | image: bmagyar/roscon2023_workshop 6 | network_mode: host 7 | init: true 8 | environment: 9 | # Wall off ROS. When this is FALSE, ROS_DOMAIN_ID is important to set 10 | - ROS_LOCALHOST_ONLY=1 11 | # Settings to make graphical programs work 12 | - DISPLAY 13 | - QT_X11_NO_MITSHM=1 14 | volumes: 15 | # Workspace mount 16 | - ../../../../ws:/home/ros/ws 17 | # Settings to make graphical programs work 18 | - /tmp/.X11-unix:/tmp/.X11-unix:ro 19 | - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority 20 | - /dev/dri:/dev/dri 21 | # this adds docker -it 22 | working_dir: /home/ros/ws 23 | stdin_open: true # docker run -i 24 | tty: true # docker run -t 25 | -------------------------------------------------------------------------------- /hercules_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(hercules_description LANGUAGES CXX) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | set(THIS_PACKAGE_INCLUDE_DEPENDS 9 | ) 10 | 11 | find_package(ament_cmake REQUIRED) 12 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 13 | find_package(${Dependency} REQUIRED) 14 | endforeach() 15 | 16 | install(DIRECTORY 17 | config launch rviz urdf 18 | DESTINATION share/${PROJECT_NAME} 19 | ) 20 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 21 | ament_package() 22 | -------------------------------------------------------------------------------- /hercules_description/config/dual_ur_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 500 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | port_force_torque_sensor_broadcaster: 9 | type: ur_controllers/ForceTorqueStateBroadcaster 10 | 11 | port_joint_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | port_forward_velocity_controller: 15 | type: velocity_controllers/JointGroupVelocityController 16 | 17 | port_gripper_controller: 18 | type: forward_command_controller/ForwardCommandController 19 | 20 | starboard_force_torque_sensor_broadcaster: 21 | type: ur_controllers/ForceTorqueStateBroadcaster 22 | 23 | starboard_joint_trajectory_controller: 24 | type: joint_trajectory_controller/JointTrajectoryController 25 | 26 | starboard_forward_velocity_controller: 27 | type: velocity_controllers/JointGroupVelocityController 28 | 29 | starboard_gripper_controller: 30 | type: forward_command_controller/ForwardCommandController 31 | 32 | 33 | ## Port 34 | port_force_torque_sensor_broadcaster: 35 | ros__parameters: 36 | sensor_name: port_tcp_fts_sensor 37 | state_interface_names: 38 | - force.x 39 | - force.y 40 | - force.z 41 | - torque.x 42 | - torque.y 43 | - torque.z 44 | frame_id: tool0 45 | topic_name: ft_data 46 | 47 | 48 | port_joint_trajectory_controller: 49 | ros__parameters: 50 | joints: 51 | - port_shoulder_pan_joint 52 | - port_shoulder_lift_joint 53 | - port_elbow_joint 54 | - port_wrist_1_joint 55 | - port_wrist_2_joint 56 | - port_wrist_3_joint 57 | command_interfaces: 58 | - position 59 | state_interfaces: 60 | - position 61 | - velocity 62 | state_publish_rate: 100.0 63 | action_monitor_rate: 20.0 64 | allow_partial_joints_goal: false 65 | constraints: 66 | stopped_velocity_tolerance: 0.2 67 | goal_time: 0.0 68 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 69 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 70 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 71 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 72 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 73 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 74 | 75 | 76 | port_forward_velocity_controller: 77 | ros__parameters: 78 | joints: 79 | - port_shoulder_pan_joint 80 | - port_shoulder_lift_joint 81 | - port_elbow_joint 82 | - port_wrist_1_joint 83 | - port_wrist_2_joint 84 | - port_wrist_3_joint 85 | interface_name: velocity 86 | 87 | port_gripper_controller: 88 | ros__parameters: 89 | joints: 90 | - port_robotiq_85_left_knuckle_joint 91 | interface_name: position 92 | 93 | 94 | # Starboard 95 | starboard_force_torque_sensor_broadcaster: 96 | ros__parameters: 97 | sensor_name: starboard_tcp_fts_sensor 98 | state_interface_names: 99 | - force.x 100 | - force.y 101 | - force.z 102 | - torque.x 103 | - torque.y 104 | - torque.z 105 | frame_id: tool0 106 | topic_name: ft_data 107 | 108 | 109 | starboard_joint_trajectory_controller: 110 | ros__parameters: 111 | joints: 112 | - starboard_shoulder_pan_joint 113 | - starboard_shoulder_lift_joint 114 | - starboard_elbow_joint 115 | - starboard_wrist_1_joint 116 | - starboard_wrist_2_joint 117 | - starboard_wrist_3_joint 118 | command_interfaces: 119 | - position 120 | state_interfaces: 121 | - position 122 | - velocity 123 | state_publish_rate: 100.0 124 | action_monitor_rate: 20.0 125 | allow_partial_joints_goal: false 126 | constraints: 127 | stopped_velocity_tolerance: 0.2 128 | goal_time: 0.0 129 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 130 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 131 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 132 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 133 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 134 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 135 | 136 | 137 | starboard_forward_velocity_controller: 138 | ros__parameters: 139 | joints: 140 | - starboard_shoulder_pan_joint 141 | - starboard_shoulder_lift_joint 142 | - starboard_elbow_joint 143 | - starboard_wrist_1_joint 144 | - starboard_wrist_2_joint 145 | - starboard_wrist_3_joint 146 | interface_name: velocity 147 | 148 | starboard_gripper_controller: 149 | ros__parameters: 150 | joints: 151 | - starboard_robotiq_85_left_knuckle_joint 152 | interface_name: position 153 | -------------------------------------------------------------------------------- /hercules_description/config/hercules_controllers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | update_rate: 500 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | base_velocity_controller: 9 | type: diff_drive_controller/DiffDriveController 10 | 11 | port_force_torque_sensor_broadcaster: 12 | type: ur_controllers/ForceTorqueStateBroadcaster 13 | 14 | port_joint_trajectory_controller: 15 | type: joint_trajectory_controller/JointTrajectoryController 16 | 17 | port_forward_velocity_controller: 18 | type: velocity_controllers/JointGroupVelocityController 19 | 20 | port_gripper_controller: 21 | type: forward_command_controller/ForwardCommandController 22 | 23 | starboard_force_torque_sensor_broadcaster: 24 | type: ur_controllers/ForceTorqueStateBroadcaster 25 | 26 | starboard_joint_trajectory_controller: 27 | type: joint_trajectory_controller/JointTrajectoryController 28 | 29 | starboard_forward_velocity_controller: 30 | type: velocity_controllers/JointGroupVelocityController 31 | 32 | starboard_gripper_controller: 33 | type: forward_command_controller/ForwardCommandController 34 | 35 | 36 | # Base 37 | base_velocity_controller: 38 | ros__parameters: 39 | left_wheel_names: [ "front_left_wheel_joint", "rear_left_wheel_joint" ] 40 | right_wheel_names: [ "front_right_wheel_joint", "rear_right_wheel_joint" ] 41 | 42 | wheel_separation: 0.5120 #0.1 # 0.256 # 0.5120 43 | wheels_per_side: 2 # actually 2, but both are controlled by 1 signal 44 | wheel_radius: 0.1651 45 | 46 | wheel_separation_multiplier: 1.0 47 | left_wheel_radius_multiplier: 1.0 48 | right_wheel_radius_multiplier: 1.0 49 | 50 | publish_rate: 50.0 51 | odom_frame_id: odom 52 | base_frame_id: base_link 53 | pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] 54 | twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] 55 | 56 | open_loop: false 57 | enable_odom_tf: true 58 | 59 | cmd_vel_timeout: 0.5 60 | #publish_limited_velocity: true 61 | use_stamped_vel: false 62 | #velocity_rolling_window_size: 10 63 | 64 | # Preserve turning radius when limiting speed/acceleration/jerk 65 | preserve_turning_radius: true 66 | 67 | # Publish limited velocity 68 | publish_cmd: true 69 | 70 | # Publish wheel data 71 | publish_wheel_data: true 72 | 73 | # Velocity and acceleration limits 74 | # Whenever a min_* is unspecified, default to -max_* 75 | linear.x.has_velocity_limits: true 76 | linear.x.has_acceleration_limits: true 77 | linear.x.has_jerk_limits: false 78 | linear.x.max_velocity: 1.0 79 | linear.x.min_velocity: -1.0 80 | linear.x.max_acceleration: 1.0 81 | linear.x.max_jerk: 0.0 82 | linear.x.min_jerk: 0.0 83 | 84 | angular.z.has_velocity_limits: true 85 | angular.z.has_acceleration_limits: true 86 | angular.z.has_jerk_limits: false 87 | angular.z.max_velocity: 1.0 88 | angular.z.min_velocity: -1.0 89 | angular.z.max_acceleration: 1.0 90 | angular.z.min_acceleration: -1.0 91 | angular.z.max_jerk: 0.0 92 | angular.z.min_jerk: 0.0 93 | 94 | 95 | # Port 96 | port_force_torque_sensor_broadcaster: 97 | ros__parameters: 98 | sensor_name: port_tcp_fts_sensor 99 | state_interface_names: 100 | - force.x 101 | - force.y 102 | - force.z 103 | - torque.x 104 | - torque.y 105 | - torque.z 106 | frame_id: port_tool0 107 | topic_name: ft_data 108 | 109 | 110 | port_joint_trajectory_controller: 111 | ros__parameters: 112 | joints: 113 | - port_shoulder_pan_joint 114 | - port_shoulder_lift_joint 115 | - port_elbow_joint 116 | - port_wrist_1_joint 117 | - port_wrist_2_joint 118 | - port_wrist_3_joint 119 | command_interfaces: 120 | - position 121 | state_interfaces: 122 | - position 123 | - velocity 124 | state_publish_rate: 100.0 125 | action_monitor_rate: 20.0 126 | allow_partial_joints_goal: false 127 | constraints: 128 | stopped_velocity_tolerance: 0.2 129 | goal_time: 0.0 130 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 131 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 132 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 133 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 134 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 135 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 136 | 137 | 138 | port_forward_velocity_controller: 139 | ros__parameters: 140 | joints: 141 | - port_shoulder_pan_joint 142 | - port_shoulder_lift_joint 143 | - port_elbow_joint 144 | - port_wrist_1_joint 145 | - port_wrist_2_joint 146 | - port_wrist_3_joint 147 | interface_name: velocity 148 | 149 | port_gripper_controller: 150 | ros__parameters: 151 | joints: 152 | - port_robotiq_85_left_knuckle_joint 153 | interface_name: position 154 | 155 | 156 | # Starboard 157 | starboard_force_torque_sensor_broadcaster: 158 | ros__parameters: 159 | sensor_name: starboard_tcp_fts_sensor 160 | state_interface_names: 161 | - force.x 162 | - force.y 163 | - force.z 164 | - torque.x 165 | - torque.y 166 | - torque.z 167 | frame_id: starboard_tool0 168 | topic_name: ft_data 169 | 170 | starboard_gripper_controller: 171 | ros__parameters: 172 | joints: 173 | - starboard_robotiq_85_left_knuckle_joint 174 | interface_name: position 175 | 176 | starboard_joint_trajectory_controller: 177 | ros__parameters: 178 | joints: 179 | - starboard_shoulder_pan_joint 180 | - starboard_shoulder_lift_joint 181 | - starboard_elbow_joint 182 | - starboard_wrist_1_joint 183 | - starboard_wrist_2_joint 184 | - starboard_wrist_3_joint 185 | command_interfaces: 186 | - position 187 | state_interfaces: 188 | - position 189 | - velocity 190 | state_publish_rate: 100.0 191 | action_monitor_rate: 20.0 192 | allow_partial_joints_goal: false 193 | constraints: 194 | stopped_velocity_tolerance: 0.2 195 | goal_time: 0.0 196 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 197 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 198 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 199 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 200 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 201 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 202 | 203 | 204 | starboard_forward_velocity_controller: 205 | ros__parameters: 206 | joints: 207 | - starboard_shoulder_pan_joint 208 | - starboard_shoulder_lift_joint 209 | - starboard_elbow_joint 210 | - starboard_wrist_1_joint 211 | - starboard_wrist_2_joint 212 | - starboard_wrist_3_joint 213 | interface_name: velocity 214 | -------------------------------------------------------------------------------- /hercules_description/config/initial_positions_port_arm.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: -1.57 2 | shoulder_lift_joint: 3.14 3 | elbow_joint: 0.0 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: 0.0 6 | wrist_3_joint: 0.0 7 | -------------------------------------------------------------------------------- /hercules_description/config/initial_positions_starboard_arm.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 1.57 2 | shoulder_lift_joint: 0.0 3 | elbow_joint: 0.0 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: 0.0 6 | wrist_3_joint: 0.0 7 | -------------------------------------------------------------------------------- /hercules_description/config/test_dual_goal_publishers_config.yaml: -------------------------------------------------------------------------------- 1 | publisher_port_joint_trajectory_controller: 2 | ros__parameters: 3 | 4 | controller_name: "port_joint_trajectory_controller" 5 | wait_sec_between_publish: 6 6 | 7 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 8 | pos1: 9 | positions: [-1.57, 3.14, 0.785, 0.785, 0.785, 0.785] 10 | pos2: 11 | positions: [-1.57, 3.925, 0.0, -1.57, 0.0, -0.785] 12 | pos3: 13 | positions: [-1.57, 3.14, 0.0, 0.0, -0.785, 0.0] 14 | pos4: 15 | positions: [-1.57, 3.14, 0.0, -1.57, 0.0, 0.0] 16 | 17 | joints: 18 | - port_shoulder_pan_joint 19 | - port_shoulder_lift_joint 20 | - port_elbow_joint 21 | - port_wrist_1_joint 22 | - port_wrist_2_joint 23 | - port_wrist_3_joint 24 | 25 | check_starting_point: false 26 | starting_point_limits: 27 | port_shoulder_pan_joint: [-0.1,0.1] 28 | port_shoulder_lift_joint: [-1.6,-1.5] 29 | port_elbow_joint: [-0.1,0.1] 30 | port_wrist_1_joint: [-1.6,-1.5] 31 | port_wrist_2_joint: [-0.1,0.1] 32 | port_wrist_3_joint: [-0.1,0.1] 33 | 34 | 35 | publisher_starboard_joint_trajectory_controller: 36 | ros__parameters: 37 | 38 | controller_name: "starboard_joint_trajectory_controller" 39 | wait_sec_between_publish: 6 40 | 41 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 42 | pos1: 43 | positions: [1.57, -1.57, 0.785, 0.785, 0.785, 0.785] 44 | pos2: 45 | positions: [1.57, -0.785, 0.0, -0.785, 0.0, 0.785] 46 | pos3: 47 | positions: [1.57, -1.57, 0.0, 0.0, -0.785, 0.0] 48 | pos4: 49 | positions: [1.57, 0.0, 0.0, -1.57, 0.0, 0.0] 50 | 51 | joints: 52 | - starboard_shoulder_pan_joint 53 | - starboard_shoulder_lift_joint 54 | - starboard_elbow_joint 55 | - starboard_wrist_1_joint 56 | - starboard_wrist_2_joint 57 | - starboard_wrist_3_joint 58 | 59 | check_starting_point: false 60 | starting_point_limits: 61 | starboard_shoulder_pan_joint: [-0.1,0.1] 62 | starboard_shoulder_lift_joint: [-1.6,-1.5] 63 | starboard_elbow_joint: [-0.1,0.1] 64 | starboard_wrist_1_joint: [-1.6,-1.5] 65 | starboard_wrist_2_joint: [-0.1,0.1] 66 | starboard_wrist_3_joint: [-0.1,0.1] 67 | 68 | 69 | publisher_port_gripper_controller: 70 | ros__parameters: 71 | 72 | publish_topic: "/port_gripper_controller/commands" 73 | wait_sec_between_publish: 1 74 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 75 | pos1: [0.0] 76 | pos2: [0.23] 77 | pos3: [0.8] 78 | pos4: [0.53] 79 | 80 | 81 | publisher_starboard_gripper_controller: 82 | ros__parameters: 83 | 84 | publish_topic: "/starboard_gripper_controller/commands" 85 | wait_sec_between_publish: 1 86 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 87 | pos1: [0.8] 88 | pos4: [0.53] 89 | pos3: [0.0] 90 | pos2: [0.23] 91 | -------------------------------------------------------------------------------- /hercules_description/launch/dual_ur_sim_control.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Dr. Denis Stogl 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ( 19 | DeclareLaunchArgument, 20 | ExecuteProcess, 21 | IncludeLaunchDescription, 22 | OpaqueFunction, 23 | RegisterEventHandler, 24 | ) 25 | from launch.conditions import IfCondition, UnlessCondition 26 | from launch.event_handlers import OnProcessExit 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 29 | from launch_ros.actions import Node 30 | from launch_ros.substitutions import FindPackageShare 31 | 32 | # For port/starboard nomenclature see: https://en.wikipedia.org/wiki/Port_and_starboard 33 | 34 | 35 | def launch_setup(context, *args, **kwargs): 36 | name = LaunchConfiguration("name") 37 | # Simulation arguments 38 | sim_gazebo_classic = LaunchConfiguration("sim_gazebo_classic") 39 | sim_gazebo = LaunchConfiguration("sim_gazebo") 40 | # ros2_control arguments 41 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 42 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 43 | # Initialize Arguments 44 | ur_type = LaunchConfiguration("ur_type") 45 | safety_limits = LaunchConfiguration("safety_limits") 46 | safety_pos_margin = LaunchConfiguration("safety_pos_margin") 47 | safety_k_position = LaunchConfiguration("safety_k_position") 48 | # General arguments 49 | runtime_config_package = LaunchConfiguration("runtime_config_package") 50 | controllers_file = LaunchConfiguration("controllers_file") 51 | description_package = LaunchConfiguration("description_package") 52 | description_file = LaunchConfiguration("description_file") 53 | prefix = LaunchConfiguration("prefix") 54 | start_joint_controller = LaunchConfiguration("start_joint_controller") 55 | port_initial_joint_controller = LaunchConfiguration("port_initial_joint_controller") 56 | starboard_initial_joint_controller = LaunchConfiguration("starboard_initial_joint_controller") 57 | launch_rviz = LaunchConfiguration("launch_rviz") 58 | 59 | joint_limit_params = PathJoinSubstitution( 60 | [FindPackageShare("ur_description"), "config", ur_type, "joint_limits.yaml"] 61 | ) 62 | kinematics_params = PathJoinSubstitution( 63 | [FindPackageShare("ur_description"), "config", ur_type, "default_kinematics.yaml"] 64 | ) 65 | physical_params = PathJoinSubstitution( 66 | [FindPackageShare("ur_description"), "config", ur_type, "physical_parameters.yaml"] 67 | ) 68 | visual_params = PathJoinSubstitution( 69 | [FindPackageShare("ur_description"), "config", ur_type, "visual_parameters.yaml"] 70 | ) 71 | 72 | initial_joint_controllers = PathJoinSubstitution( 73 | [FindPackageShare(runtime_config_package), "config", "examples", controllers_file] 74 | ) 75 | 76 | rviz_config_file = PathJoinSubstitution( 77 | [FindPackageShare(description_package), "rviz", "view_robot.rviz"] 78 | ) 79 | 80 | robot_description_content = Command( 81 | [ 82 | PathJoinSubstitution([FindExecutable(name="xacro")]), 83 | " ", 84 | PathJoinSubstitution( 85 | [FindPackageShare(description_package), "urdf", description_file] 86 | ), 87 | " ", 88 | "name:=", 89 | name, 90 | " ", 91 | "prefix:=", 92 | prefix, 93 | # Simulation parameters 94 | " ", 95 | "sim_gazebo_classic:=", 96 | sim_gazebo_classic, 97 | " ", 98 | "sim_gazebo:=", 99 | sim_gazebo, 100 | " ", 101 | "simulation_controllers:=", 102 | initial_joint_controllers, 103 | " ", 104 | # ros2_control parameters 105 | "mock_sensor_commands:=", 106 | mock_sensor_commands, 107 | " ", 108 | # UR parameters 109 | "joint_limit_params:=", 110 | joint_limit_params, 111 | " ", 112 | "kinematics_params:=", 113 | kinematics_params, 114 | " ", 115 | "physical_params:=", 116 | physical_params, 117 | " ", 118 | "visual_params:=", 119 | visual_params, 120 | " ", 121 | "safety_limits:=", 122 | safety_limits, 123 | " ", 124 | "safety_pos_margin:=", 125 | safety_pos_margin, 126 | " ", 127 | "safety_k_position:=", 128 | safety_k_position, 129 | ] 130 | ) 131 | robot_description = {"robot_description": robot_description_content} 132 | 133 | robot_state_publisher_node = Node( 134 | package="robot_state_publisher", 135 | executable="robot_state_publisher", 136 | output="both", 137 | parameters=[{"use_sim_time": True}, robot_description], 138 | ) 139 | 140 | rviz_node = Node( 141 | package="rviz2", 142 | executable="rviz2", 143 | name="rviz2", 144 | output="log", 145 | arguments=["-d", rviz_config_file], 146 | condition=IfCondition(launch_rviz), 147 | ) 148 | 149 | joint_state_broadcaster_spawner = Node( 150 | package="controller_manager", 151 | executable="spawner", 152 | arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], 153 | ) 154 | 155 | # Delay rviz start after `joint_state_broadcaster` 156 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 157 | event_handler=OnProcessExit( 158 | target_action=joint_state_broadcaster_spawner, 159 | on_exit=[rviz_node], 160 | ) 161 | ) 162 | 163 | # Port 164 | 165 | # There may be other controllers of the joints, but this is the initially-started one 166 | port_initial_joint_controller_spawner_started = Node( 167 | package="controller_manager", 168 | executable="spawner", 169 | arguments=[port_initial_joint_controller, "-c", "/controller_manager"], 170 | condition=IfCondition(start_joint_controller), 171 | ) 172 | port_initial_joint_controller_spawner_stopped = Node( 173 | package="controller_manager", 174 | executable="spawner", 175 | arguments=[port_initial_joint_controller, "-c", "/controller_manager", "--stopped"], 176 | condition=UnlessCondition(start_joint_controller), 177 | ) 178 | 179 | port_gripper_controller_spawner = Node( 180 | package="controller_manager", 181 | executable="spawner", 182 | arguments=["port_gripper_controller", "-c", "/controller_manager"], 183 | ) 184 | 185 | # Starboard 186 | 187 | # There may be other controllers of the joints, but this is the initially-started one 188 | starboard_initial_joint_controller_spawner_started = Node( 189 | package="controller_manager", 190 | executable="spawner", 191 | arguments=[starboard_initial_joint_controller, "-c", "/controller_manager"], 192 | condition=IfCondition(start_joint_controller), 193 | ) 194 | starboard_initial_joint_controller_spawner_stopped = Node( 195 | package="controller_manager", 196 | executable="spawner", 197 | arguments=[starboard_initial_joint_controller, "-c", "/controller_manager", "--stopped"], 198 | condition=UnlessCondition(start_joint_controller), 199 | ) 200 | 201 | starboard_gripper_controller_spawner = Node( 202 | package="controller_manager", 203 | executable="spawner", 204 | arguments=["starboard_gripper_controller", "-c", "/controller_manager"], 205 | ) 206 | 207 | # ros2_control_node 208 | robot_controllers = PathJoinSubstitution( 209 | [FindPackageShare(runtime_config_package), "config", controllers_file] 210 | ) 211 | ros2_control_node = Node( 212 | package="controller_manager", 213 | executable="ros2_control_node", 214 | parameters=[robot_description, robot_controllers], 215 | output={ 216 | "stdout": "screen", 217 | "stderr": "screen", 218 | }, 219 | condition=IfCondition(use_mock_hardware), 220 | ) 221 | 222 | # Gazebo nodes 223 | gzserver = ExecuteProcess( 224 | cmd=["gzserver", "-s", "libgazebo_ros_init.so", "-s", "libgazebo_ros_factory.so", ""], 225 | output="screen", 226 | condition=IfCondition(sim_gazebo_classic), 227 | ) 228 | 229 | # Gazebo client 230 | gzclient = ExecuteProcess( 231 | cmd=["gzclient"], 232 | output="screen", 233 | condition=IfCondition(sim_gazebo_classic), 234 | ) 235 | 236 | # Spawn robot 237 | gazebo_spawn_robot = Node( 238 | package="gazebo_ros", 239 | executable="spawn_entity.py", 240 | name="spawn_ur", 241 | arguments=["-entity", name, "-topic", "robot_description"], 242 | output="screen", 243 | condition=IfCondition(sim_gazebo_classic), 244 | ) 245 | 246 | # Ignition nodes 247 | ignition_spawn_entity = Node( 248 | package="ros_ign_gazebo", 249 | executable="create", 250 | output="screen", 251 | arguments=[ 252 | "-string", 253 | robot_description_content, 254 | "-name", 255 | name, 256 | "-allow_renaming", 257 | "true", 258 | ], 259 | condition=IfCondition(sim_gazebo), 260 | ) 261 | 262 | ignition_launch_description = IncludeLaunchDescription( 263 | PythonLaunchDescriptionSource( 264 | [FindPackageShare("ros_ign_gazebo"), "/launch/ign_gazebo.launch.py"] 265 | ), 266 | launch_arguments={"ign_args": " -r -v 3 empty.sdf"}.items(), 267 | condition=IfCondition(sim_gazebo), 268 | ) 269 | 270 | nodes_to_start = [ 271 | robot_state_publisher_node, 272 | delay_rviz_after_joint_state_broadcaster_spawner, 273 | joint_state_broadcaster_spawner, 274 | port_initial_joint_controller_spawner_stopped, 275 | port_initial_joint_controller_spawner_started, 276 | port_gripper_controller_spawner, 277 | starboard_initial_joint_controller_spawner_stopped, 278 | starboard_initial_joint_controller_spawner_started, 279 | starboard_gripper_controller_spawner, 280 | ros2_control_node, 281 | gzserver, 282 | gzclient, 283 | gazebo_spawn_robot, 284 | ignition_spawn_entity, 285 | ignition_launch_description, 286 | ] 287 | 288 | return nodes_to_start 289 | 290 | 291 | def generate_launch_description(): 292 | declared_arguments = [] 293 | declared_arguments.append( 294 | DeclareLaunchArgument( 295 | "name", 296 | description="Robot name", 297 | default_value="dual_ur", 298 | ) 299 | ) 300 | # Simulation specific arguments 301 | declared_arguments.append( 302 | DeclareLaunchArgument( 303 | "sim_gazebo_classic", 304 | default_value="false", 305 | description="Use Gazebo Classic for simulation", 306 | ) 307 | ) 308 | declared_arguments.append( 309 | DeclareLaunchArgument( 310 | "sim_gazebo", 311 | default_value="false", 312 | description="Use Ignition for simulation", 313 | ) 314 | ) 315 | # UR specific arguments 316 | declared_arguments.append( 317 | DeclareLaunchArgument( 318 | "ur_type", 319 | description="Type/series of used UR robot.", 320 | choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], 321 | default_value="ur5e", 322 | ) 323 | ) 324 | declared_arguments.append( 325 | DeclareLaunchArgument( 326 | "robot_ip", 327 | description="IP address by which the robot can be reached.", 328 | default_value="xxx.yyy.zzz.www", 329 | ) 330 | ) 331 | declared_arguments.append( 332 | DeclareLaunchArgument( 333 | "safety_limits", 334 | default_value="true", 335 | description="Enables the safety limits controller if true.", 336 | ) 337 | ) 338 | declared_arguments.append( 339 | DeclareLaunchArgument( 340 | "safety_pos_margin", 341 | default_value="0.15", 342 | description="The margin to lower and upper limits in the safety controller.", 343 | ) 344 | ) 345 | declared_arguments.append( 346 | DeclareLaunchArgument( 347 | "safety_k_position", 348 | default_value="20", 349 | description="k-position factor in the safety controller.", 350 | ) 351 | ) 352 | # General arguments 353 | declared_arguments.append( 354 | DeclareLaunchArgument( 355 | "runtime_config_package", 356 | default_value="hercules_description", 357 | description='Package with the controller\'s configuration in "config" folder. \ 358 | Usually the argument is not set, it enables use of a custom setup.', 359 | ) 360 | ) 361 | declared_arguments.append( 362 | DeclareLaunchArgument( 363 | "controllers_file", 364 | default_value="dual_ur_controllers.yaml", 365 | description="YAML file with the controllers configuration.", 366 | ) 367 | ) 368 | declared_arguments.append( 369 | DeclareLaunchArgument( 370 | "description_package", 371 | default_value="hercules_description", 372 | description="Description package with robot URDF/XACRO files. Usually the argument \ 373 | is not set, it enables use of a custom description.", 374 | ) 375 | ) 376 | declared_arguments.append( 377 | DeclareLaunchArgument( 378 | "description_file", 379 | default_value="dual_ur.urdf.xacro", 380 | description="URDF/XACRO description file with the robot.", 381 | ) 382 | ) 383 | declared_arguments.append( 384 | DeclareLaunchArgument( 385 | "prefix", 386 | default_value="", 387 | description="Prefix of the joint names, useful for \ 388 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 389 | have to be updated.", 390 | ) 391 | ) 392 | declared_arguments.append( 393 | DeclareLaunchArgument( 394 | "use_mock_hardware", 395 | default_value="true", 396 | description="Start robot with hardware mocking in ros2_control.", 397 | ) 398 | ) 399 | declared_arguments.append( 400 | DeclareLaunchArgument( 401 | "mock_sensor_commands", 402 | default_value="false", 403 | description="Enable fake command interfaces for sensors used for simple simulations. \ 404 | Used only if 'use_mock_hardware' parameter is true.", 405 | ) 406 | ) 407 | declared_arguments.append( 408 | DeclareLaunchArgument( 409 | "start_joint_controller", 410 | default_value="true", 411 | description="Enable headless mode for robot control", 412 | ) 413 | ) 414 | declared_arguments.append( 415 | DeclareLaunchArgument( 416 | "port_initial_joint_controller", 417 | default_value="port_joint_trajectory_controller", 418 | description="Robot controller to start.", 419 | ) 420 | ) 421 | declared_arguments.append( 422 | DeclareLaunchArgument( 423 | "starboard_initial_joint_controller", 424 | default_value="starboard_joint_trajectory_controller", 425 | description="Robot controller to start.", 426 | ) 427 | ) 428 | declared_arguments.append( 429 | DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") 430 | ) 431 | 432 | return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) 433 | -------------------------------------------------------------------------------- /hercules_description/launch/hercules_controllers.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Dr. Denis Stogl 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ( 19 | DeclareLaunchArgument, 20 | OpaqueFunction, 21 | ) 22 | from launch.conditions import IfCondition, UnlessCondition 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import Node 25 | 26 | # For port/starboard nomenclature see: https://en.wikipedia.org/wiki/Port_and_starboard 27 | 28 | 29 | def launch_setup(context, *args, **kwargs): 30 | # General arguments 31 | start_arm_controllers = LaunchConfiguration("start_arm_controllers") 32 | base_controller = LaunchConfiguration("base_controller") 33 | port_joint_controller = LaunchConfiguration("port_joint_controller") 34 | port_gripper_controller = LaunchConfiguration("port_gripper_controller") 35 | starboard_joint_controller = LaunchConfiguration("starboard_joint_controller") 36 | starboard_gripper_controller = LaunchConfiguration("starboard_gripper_controller") 37 | 38 | controller_manager_name = LaunchConfiguration("controller_manager_name") 39 | 40 | # Base 41 | spawn_base_controller = Node( 42 | package="controller_manager", 43 | executable="spawner", 44 | arguments=[base_controller, "-c", controller_manager_name], 45 | output="screen", 46 | ) 47 | 48 | # Port 49 | 50 | # There may be other controllers of the joints, but this is the initially-started one 51 | port_joint_controller_spawner_started = Node( 52 | package="controller_manager", 53 | executable="spawner", 54 | arguments=[ 55 | port_joint_controller, 56 | "-c", 57 | controller_manager_name, 58 | ], 59 | condition=IfCondition(start_arm_controllers), 60 | ) 61 | port_joint_controller_spawner_stopped = Node( 62 | package="controller_manager", 63 | executable="spawner", 64 | arguments=[ 65 | port_joint_controller, 66 | "-c", 67 | controller_manager_name, 68 | "--stopped", 69 | ], 70 | condition=UnlessCondition(start_arm_controllers), 71 | ) 72 | port_gripper_controller_spawner = Node( 73 | package="controller_manager", 74 | executable="spawner", 75 | arguments=[port_gripper_controller, "-c", controller_manager_name], 76 | ) 77 | 78 | # Starboard 79 | 80 | # There may be other controllers of the joints, but this is the initially-started one 81 | starboard_joint_controller_spawner_started = Node( 82 | package="controller_manager", 83 | executable="spawner", 84 | arguments=[ 85 | starboard_joint_controller, 86 | "-c", 87 | controller_manager_name, 88 | ], 89 | condition=IfCondition(start_arm_controllers), 90 | ) 91 | starboard_joint_controller_spawner_stopped = Node( 92 | package="controller_manager", 93 | executable="spawner", 94 | arguments=[ 95 | starboard_joint_controller, 96 | "-c", 97 | controller_manager_name, 98 | "--stopped", 99 | ], 100 | condition=UnlessCondition(start_arm_controllers), 101 | ) 102 | starboard_gripper_controller_spawner = Node( 103 | package="controller_manager", 104 | executable="spawner", 105 | arguments=[ 106 | starboard_gripper_controller, 107 | "-c", 108 | controller_manager_name, 109 | ], 110 | ) 111 | 112 | nodes_to_start = [ 113 | spawn_base_controller, 114 | port_joint_controller_spawner_stopped, 115 | port_joint_controller_spawner_started, 116 | port_gripper_controller_spawner, 117 | starboard_joint_controller_spawner_stopped, 118 | starboard_joint_controller_spawner_started, 119 | starboard_gripper_controller_spawner, 120 | ] 121 | 122 | return nodes_to_start 123 | 124 | 125 | def generate_launch_description(): 126 | declared_arguments = [] 127 | declared_arguments.append( 128 | DeclareLaunchArgument( 129 | "controller_manager_name", 130 | description="Name of the used controller manager.", 131 | default_value="/controller_manager", 132 | ) 133 | ) 134 | declared_arguments.append( 135 | DeclareLaunchArgument( 136 | "start_arm_controllers", 137 | default_value="true", 138 | description="Start arom controllers", 139 | ) 140 | ) 141 | declared_arguments.append( 142 | DeclareLaunchArgument( 143 | "base_controller", 144 | default_value="base_velocity_controller", 145 | description="Base controller to start.", 146 | ) 147 | ) 148 | declared_arguments.append( 149 | DeclareLaunchArgument( 150 | "port_joint_controller", 151 | default_value="port_joint_trajectory_controller", 152 | description="Robot controller to start.", 153 | ) 154 | ) 155 | declared_arguments.append( 156 | DeclareLaunchArgument( 157 | "starboard_joint_controller", 158 | default_value="starboard_joint_trajectory_controller", 159 | description="Robot controller to start.", 160 | ) 161 | ) 162 | declared_arguments.append( 163 | DeclareLaunchArgument( 164 | "port_gripper_controller", 165 | default_value="port_gripper_controller", 166 | description="Gripper controller to start.", 167 | ) 168 | ) 169 | declared_arguments.append( 170 | DeclareLaunchArgument( 171 | "starboard_gripper_controller", 172 | default_value="starboard_gripper_controller", 173 | description="Gripper controller to start.", 174 | ) 175 | ) 176 | declared_arguments.append( 177 | DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") 178 | ) 179 | 180 | return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) 181 | -------------------------------------------------------------------------------- /hercules_description/launch/hercules_sim_control.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Dr. Denis Stogl 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ( 19 | DeclareLaunchArgument, 20 | ExecuteProcess, 21 | IncludeLaunchDescription, 22 | OpaqueFunction, 23 | RegisterEventHandler, 24 | ) 25 | from launch.conditions import IfCondition 26 | from launch.event_handlers import OnProcessExit 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 29 | from launch_ros.actions import Node 30 | from launch_ros.substitutions import FindPackageShare 31 | 32 | # For port/starboard nomenclature see: https://en.wikipedia.org/wiki/Port_and_starboard 33 | 34 | 35 | def launch_setup(context, *args, **kwargs): 36 | name = LaunchConfiguration("name") 37 | # Simulation arguments 38 | sim_gazebo_classic = LaunchConfiguration("sim_gazebo_classic") 39 | sim_gazebo = LaunchConfiguration("sim_gazebo") 40 | # ros2_control arguments 41 | controller_manager_namespace = LaunchConfiguration("controller_manager_namespace") 42 | controller_manager_name = LaunchConfiguration("controller_manager_name") 43 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 44 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 45 | # Initialize Arguments 46 | ur_type = LaunchConfiguration("ur_type") 47 | safety_limits = LaunchConfiguration("safety_limits") 48 | safety_pos_margin = LaunchConfiguration("safety_pos_margin") 49 | safety_k_position = LaunchConfiguration("safety_k_position") 50 | # General arguments 51 | runtime_config_package = LaunchConfiguration("runtime_config_package") 52 | controllers_file = LaunchConfiguration("controllers_file") 53 | description_package = LaunchConfiguration("description_package") 54 | description_file = LaunchConfiguration("description_file") 55 | prefix = LaunchConfiguration("prefix") 56 | start_arm_controllers = LaunchConfiguration("start_arm_controllers") 57 | odometry_frame_name = LaunchConfiguration("odometry_frame_name") 58 | map_to_odom_y_axis = LaunchConfiguration("map_to_odom_y_axis") 59 | launch_rviz = LaunchConfiguration("launch_rviz") 60 | 61 | joint_limit_params = PathJoinSubstitution( 62 | [FindPackageShare("ur_description"), "config", ur_type, "joint_limits.yaml"] 63 | ) 64 | kinematics_params = PathJoinSubstitution( 65 | [FindPackageShare("ur_description"), "config", ur_type, "default_kinematics.yaml"] 66 | ) 67 | physical_params = PathJoinSubstitution( 68 | [FindPackageShare("ur_description"), "config", ur_type, "physical_parameters.yaml"] 69 | ) 70 | visual_params = PathJoinSubstitution( 71 | [FindPackageShare("ur_description"), "config", ur_type, "visual_parameters.yaml"] 72 | ) 73 | 74 | initial_joint_controllers = PathJoinSubstitution( 75 | [FindPackageShare(runtime_config_package), "config", "examples", controllers_file] 76 | ) 77 | 78 | rviz_config_file = PathJoinSubstitution( 79 | [FindPackageShare(description_package), "rviz", "view_robot.rviz"] 80 | ) 81 | 82 | robot_description_content = Command( 83 | [ 84 | PathJoinSubstitution([FindExecutable(name="xacro")]), 85 | " ", 86 | PathJoinSubstitution( 87 | [FindPackageShare(description_package), "urdf", description_file] 88 | ), 89 | " ", 90 | "name:=", 91 | name, 92 | " ", 93 | "prefix:=", 94 | prefix, 95 | # Simulation parameters 96 | " ", 97 | "sim_gazebo_classic:=", 98 | sim_gazebo_classic, 99 | " ", 100 | "sim_gazebo:=", 101 | sim_gazebo, 102 | " ", 103 | "simulation_controllers:=", 104 | initial_joint_controllers, 105 | " ", 106 | # ros2_control parameters 107 | "use_mock_hardware:=", 108 | use_mock_hardware, 109 | " ", 110 | "mock_sensor_commands:=", 111 | mock_sensor_commands, 112 | " ", 113 | # UR parameters 114 | "joint_limit_params:=", 115 | joint_limit_params, 116 | " ", 117 | "kinematics_params:=", 118 | kinematics_params, 119 | " ", 120 | "physical_params:=", 121 | physical_params, 122 | " ", 123 | "visual_params:=", 124 | visual_params, 125 | " ", 126 | "safety_limits:=", 127 | safety_limits, 128 | " ", 129 | "safety_pos_margin:=", 130 | safety_pos_margin, 131 | " ", 132 | "safety_k_position:=", 133 | safety_k_position, 134 | # Husky parameters 135 | " ", 136 | "is_sim:=true", 137 | ] 138 | ) 139 | robot_description = {"robot_description": robot_description_content} 140 | 141 | robot_state_publisher_node = Node( 142 | package="robot_state_publisher", 143 | executable="robot_state_publisher", 144 | namespace=controller_manager_namespace, 145 | output="both", 146 | parameters=[{"use_sim_time": True}, robot_description], 147 | ) 148 | 149 | rviz_node = Node( 150 | package="rviz2", 151 | executable="rviz2", 152 | name="rviz2", 153 | output="log", 154 | arguments=["-d", rviz_config_file], 155 | condition=IfCondition(launch_rviz), 156 | ) 157 | 158 | joint_state_broadcaster_spawner = Node( 159 | package="controller_manager", 160 | executable="spawner", 161 | arguments=["joint_state_broadcaster", "--controller-manager", controller_manager_name], 162 | ) 163 | 164 | # Delay rviz start after `joint_state_broadcaster` 165 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 166 | event_handler=OnProcessExit( 167 | target_action=joint_state_broadcaster_spawner, 168 | on_exit=[rviz_node], 169 | ) 170 | ) 171 | 172 | hercules_controller_spawners = IncludeLaunchDescription( 173 | PythonLaunchDescriptionSource( 174 | [ 175 | FindPackageShare("hercules_description"), 176 | "/launch", 177 | "/hercules_controllers.launch.py", 178 | ] 179 | ), 180 | launch_arguments={ 181 | "controller_manager_name": controller_manager_name, 182 | "start_arm_controllers": start_arm_controllers, 183 | "base_controller": "base_velocity_controller", 184 | "port_joint_controller": "port_joint_trajectory_controller", 185 | "starboard_joint_controller": "starboard_joint_trajectory_controller", 186 | "port_gripper_controller": "port_gripper_controller", 187 | "starboard_gripper_controller": "starboard_gripper_controller", 188 | }.items(), 189 | ) 190 | 191 | # ros2_control_node 192 | robot_controllers = PathJoinSubstitution( 193 | [FindPackageShare(runtime_config_package), "config", controllers_file] 194 | ) 195 | ros2_control_node = Node( 196 | package="controller_manager", 197 | executable="ros2_control_node", 198 | namespace=controller_manager_namespace, 199 | parameters=[robot_description, robot_controllers], 200 | output={ 201 | "stdout": "screen", 202 | "stderr": "screen", 203 | }, 204 | condition=IfCondition(use_mock_hardware), 205 | ) 206 | 207 | map_to_odom_tf = Node( 208 | package="tf2_ros", 209 | executable="static_transform_publisher", 210 | namespace=controller_manager_namespace, 211 | name="map_to_odom", 212 | arguments=["0", map_to_odom_y_axis, "0", "0", "0", "0", "map", odometry_frame_name], 213 | output="screen", 214 | ) 215 | 216 | # Gazebo nodes 217 | gzserver = ExecuteProcess( 218 | cmd=["gzserver", "-s", "libgazebo_ros_init.so", "-s", "libgazebo_ros_factory.so", ""], 219 | output="screen", 220 | condition=IfCondition(sim_gazebo_classic), 221 | ) 222 | 223 | # Gazebo client 224 | gzclient = ExecuteProcess( 225 | cmd=["gzclient"], 226 | output="screen", 227 | condition=IfCondition(sim_gazebo_classic), 228 | ) 229 | 230 | # Spawn robot 231 | gazebo_spawn_robot = Node( 232 | package="gazebo_ros", 233 | executable="spawn_entity.py", 234 | name="spawn_ur", 235 | arguments=["-entity", name, "-topic", "robot_description"], 236 | output="screen", 237 | condition=IfCondition(sim_gazebo_classic), 238 | ) 239 | 240 | # Ignition nodes 241 | ignition_spawn_entity = Node( 242 | package="ros_ign_gazebo", 243 | executable="create", 244 | output="screen", 245 | arguments=[ 246 | "-string", 247 | robot_description_content, 248 | "-name", 249 | name, 250 | "-allow_renaming", 251 | "true", 252 | ], 253 | condition=IfCondition(sim_gazebo), 254 | ) 255 | 256 | ignition_launch_description = IncludeLaunchDescription( 257 | PythonLaunchDescriptionSource( 258 | [FindPackageShare("ros_ign_gazebo"), "/launch/ign_gazebo.launch.py"] 259 | ), 260 | launch_arguments={"ign_args": " -r -v 3 empty.sdf"}.items(), 261 | condition=IfCondition(sim_gazebo), 262 | ) 263 | 264 | nodes_to_start = [ 265 | robot_state_publisher_node, 266 | delay_rviz_after_joint_state_broadcaster_spawner, 267 | joint_state_broadcaster_spawner, 268 | hercules_controller_spawners, 269 | ros2_control_node, 270 | map_to_odom_tf, 271 | gzserver, 272 | gzclient, 273 | gazebo_spawn_robot, 274 | ignition_spawn_entity, 275 | ignition_launch_description, 276 | ] 277 | 278 | return nodes_to_start 279 | 280 | 281 | def generate_launch_description(): 282 | declared_arguments = [] 283 | declared_arguments.append( 284 | DeclareLaunchArgument( 285 | "name", 286 | description="Robot name", 287 | default_value="hercules", 288 | ) 289 | ) 290 | # Simulation specific arguments 291 | declared_arguments.append( 292 | DeclareLaunchArgument( 293 | "sim_gazebo_classic", 294 | default_value="false", 295 | description="Use Gazebo Classic for simulation", 296 | ) 297 | ) 298 | declared_arguments.append( 299 | DeclareLaunchArgument( 300 | "sim_gazebo", 301 | default_value="false", 302 | description="Use Ignition for simulation", 303 | ) 304 | ) 305 | # UR specific arguments 306 | declared_arguments.append( 307 | DeclareLaunchArgument( 308 | "ur_type", 309 | description="Type/series of used UR robot.", 310 | choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], 311 | default_value="ur5e", 312 | ) 313 | ) 314 | declared_arguments.append( 315 | DeclareLaunchArgument( 316 | "safety_limits", 317 | default_value="true", 318 | description="Enables the safety limits controller if true.", 319 | ) 320 | ) 321 | declared_arguments.append( 322 | DeclareLaunchArgument( 323 | "safety_pos_margin", 324 | default_value="0.15", 325 | description="The margin to lower and upper limits in the safety controller.", 326 | ) 327 | ) 328 | declared_arguments.append( 329 | DeclareLaunchArgument( 330 | "safety_k_position", 331 | default_value="20", 332 | description="k-position factor in the safety controller.", 333 | ) 334 | ) 335 | # General arguments 336 | declared_arguments.append( 337 | DeclareLaunchArgument( 338 | "runtime_config_package", 339 | default_value="hercules_description", 340 | description='Package with the controller\'s configuration in "config" folder. \ 341 | Usually the argument is not set, it enables use of a custom setup.', 342 | ) 343 | ) 344 | declared_arguments.append( 345 | DeclareLaunchArgument( 346 | "controllers_file", 347 | default_value="hercules_controllers.yaml", 348 | description="YAML file with the controllers configuration.", 349 | ) 350 | ) 351 | declared_arguments.append( 352 | DeclareLaunchArgument( 353 | "description_package", 354 | default_value="hercules_description", 355 | description="Description package with robot URDF/XACRO files. Usually the argument \ 356 | is not set, it enables use of a custom description.", 357 | ) 358 | ) 359 | declared_arguments.append( 360 | DeclareLaunchArgument( 361 | "description_file", 362 | default_value="hercules.urdf.xacro", 363 | description="URDF/XACRO description file with the robot.", 364 | ) 365 | ) 366 | declared_arguments.append( 367 | DeclareLaunchArgument( 368 | "prefix", 369 | default_value='""', 370 | description="Prefix of the joint names, useful for \ 371 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 372 | have to be updated.", 373 | ) 374 | ) 375 | declared_arguments.append( 376 | DeclareLaunchArgument( 377 | "controller_manager_namespace", 378 | default_value="/", 379 | description="Namespace of the controller manager and ros2_control.", 380 | ) 381 | ) 382 | declared_arguments.append( 383 | DeclareLaunchArgument( 384 | "controller_manager_name", 385 | default_value="controller_manager", 386 | description="Name of the controller manager", 387 | ) 388 | ) 389 | declared_arguments.append( 390 | DeclareLaunchArgument( 391 | "use_mock_hardware", 392 | default_value="true", 393 | description="Start robot with fake hardware mirroring command to its states.", 394 | ) 395 | ) 396 | declared_arguments.append( 397 | DeclareLaunchArgument( 398 | "mock_sensor_commands", 399 | default_value="false", 400 | description="Enable fake command interfaces for sensors used for simple simulations. \ 401 | Used only if 'use_mock_hardware' parameter is true.", 402 | ) 403 | ) 404 | declared_arguments.append( 405 | DeclareLaunchArgument( 406 | "start_arm_controllers", 407 | default_value="true", 408 | description="Start arom controllers", 409 | ) 410 | ) 411 | declared_arguments.append( 412 | DeclareLaunchArgument( 413 | "odometry_frame_name", 414 | default_value="odom", 415 | description="Odometry frame of the robot to have correct transform to map.", 416 | ) 417 | ) 418 | declared_arguments.append( 419 | DeclareLaunchArgument( 420 | "map_to_odom_y_axis", 421 | default_value="0", 422 | description="Value of the y-Axis for map to odom transformation.", 423 | ) 424 | ) 425 | declared_arguments.append( 426 | DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") 427 | ) 428 | 429 | return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) 430 | -------------------------------------------------------------------------------- /hercules_description/launch/test_dual_ur_controllers.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Dr. Denis Stogl 16 | # 17 | # Description: After a robot has been loaded, this will execute a series of trajectories. 18 | 19 | from launch import LaunchDescription 20 | from launch.substitutions import PathJoinSubstitution 21 | from launch_ros.actions import Node 22 | from launch_ros.substitutions import FindPackageShare 23 | 24 | 25 | def generate_launch_description(): 26 | position_goals = PathJoinSubstitution( 27 | [ 28 | FindPackageShare("hercules_description"), 29 | "config", 30 | "test_dual_goal_publishers_config.yaml", 31 | ] 32 | ) 33 | 34 | return LaunchDescription( 35 | [ 36 | Node( 37 | package="ros2_controllers_test_nodes", 38 | executable="publisher_joint_trajectory_controller", 39 | name="publisher_port_joint_trajectory_controller", 40 | parameters=[position_goals], 41 | output={ 42 | "stdout": "screen", 43 | "stderr": "screen", 44 | }, 45 | ), 46 | Node( 47 | package="ros2_controllers_test_nodes", 48 | executable="publisher_joint_trajectory_controller", 49 | name="publisher_starboard_joint_trajectory_controller", 50 | parameters=[position_goals], 51 | output={ 52 | "stdout": "screen", 53 | "stderr": "screen", 54 | }, 55 | ), 56 | Node( 57 | package="ros2_controllers_test_nodes", 58 | executable="publisher_forward_position_controller", 59 | name="publisher_port_gripper_controller", 60 | parameters=[position_goals], 61 | output={ 62 | "stdout": "screen", 63 | "stderr": "screen", 64 | }, 65 | ), 66 | Node( 67 | package="ros2_controllers_test_nodes", 68 | executable="publisher_forward_position_controller", 69 | name="publisher_starboard_gripper_controller", 70 | parameters=[position_goals], 71 | output={ 72 | "stdout": "screen", 73 | "stderr": "screen", 74 | }, 75 | ), 76 | ] 77 | ) 78 | -------------------------------------------------------------------------------- /hercules_description/launch/test_hercules_controllers.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Author: Dr. Denis Stogl 16 | # 17 | # Description: After a robot has been loaded, this will execute a series of trajectories. 18 | 19 | from launch import LaunchDescription 20 | from launch.substitutions import PathJoinSubstitution 21 | from launch.actions import ExecuteProcess 22 | from launch_ros.actions import Node 23 | from launch_ros.substitutions import FindPackageShare 24 | 25 | 26 | def generate_launch_description(): 27 | position_goals = PathJoinSubstitution( 28 | [ 29 | FindPackageShare("hercules_description"), 30 | "config", 31 | "test_dual_goal_publishers_config.yaml", 32 | ] 33 | ) 34 | 35 | return LaunchDescription( 36 | [ 37 | Node( 38 | package="ros2_controllers_test_nodes", 39 | executable="publisher_joint_trajectory_controller", 40 | name="publisher_port_joint_trajectory_controller", 41 | parameters=[position_goals], 42 | output={ 43 | "stdout": "screen", 44 | "stderr": "screen", 45 | }, 46 | ), 47 | Node( 48 | package="ros2_controllers_test_nodes", 49 | executable="publisher_joint_trajectory_controller", 50 | name="publisher_starboard_joint_trajectory_controller", 51 | parameters=[position_goals], 52 | output={ 53 | "stdout": "screen", 54 | "stderr": "screen", 55 | }, 56 | ), 57 | Node( 58 | package="ros2_controllers_test_nodes", 59 | executable="publisher_forward_position_controller", 60 | name="publisher_port_gripper_controller", 61 | parameters=[position_goals], 62 | output={ 63 | "stdout": "screen", 64 | "stderr": "screen", 65 | }, 66 | ), 67 | Node( 68 | package="ros2_controllers_test_nodes", 69 | executable="publisher_forward_position_controller", 70 | name="publisher_starboard_gripper_controller", 71 | parameters=[position_goals], 72 | output={ 73 | "stdout": "screen", 74 | "stderr": "screen", 75 | }, 76 | ), 77 | ExecuteProcess( 78 | cmd=[ 79 | "ros2 topic pub -r 10 /base_velocity_controller/cmd_vel_unstamped geometry_msgs/msg/Twist '{linear: {x: 0.4, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}'" 80 | ], 81 | shell=True, 82 | ), 83 | ] 84 | ) 85 | -------------------------------------------------------------------------------- /hercules_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | hercules_description 5 | 0.0.0 6 | Description files of Hercules robot used in the workshop. The name and design of the robot is curtosy for Brett Aldrich (Robosoft). 7 | Dr. Denis Stogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | ros2_controllers_test_nodes 13 | 14 | 15 | ament_cmake 16 | 17 | 18 | -------------------------------------------------------------------------------- /hercules_description/rviz/view_robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 87 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 1026 10 | - Class: rviz_common/Selection 11 | Name: Selection 12 | - Class: rviz_common/Tool Properties 13 | Expanded: 14 | - /2D Goal Pose1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz_common/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz_default_plugins/RobotModel 51 | Collision Enabled: false 52 | Description File: "" 53 | Description Source: Topic 54 | Description Topic: 55 | Depth: 5 56 | Durability Policy: Volatile 57 | History Policy: Keep Last 58 | Reliability Policy: Reliable 59 | Value: /robot_description 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | hercules_1_base_footprint: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | hercules_1_base_link: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | hercules_1_front_bumper_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | hercules_1_front_left_wheel: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | hercules_1_front_right_wheel: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | hercules_1_gps_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | hercules_1_imu_link: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | hercules_1_inertial_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | hercules_1_manipulator_fixture_box: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | hercules_1_port_base: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | hercules_1_port_base_link: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | hercules_1_port_base_link_inertia: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | hercules_1_port_flange: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | hercules_1_port_forearm_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | hercules_1_port_ft_frame: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | hercules_1_port_robotiq_85_base_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | hercules_1_port_robotiq_85_left_finger_link: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | hercules_1_port_robotiq_85_left_finger_tip_link: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | hercules_1_port_robotiq_85_left_inner_knuckle_link: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | hercules_1_port_robotiq_85_left_knuckle_link: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | hercules_1_port_robotiq_85_right_finger_link: 160 | Alpha: 1 161 | Show Axes: false 162 | Show Trail: false 163 | Value: true 164 | hercules_1_port_robotiq_85_right_finger_tip_link: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | Value: true 169 | hercules_1_port_robotiq_85_right_inner_knuckle_link: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | hercules_1_port_robotiq_85_right_knuckle_link: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | hercules_1_port_shoulder_link: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | hercules_1_port_tool0: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | hercules_1_port_upper_arm_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | hercules_1_port_wrist_1_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | hercules_1_port_wrist_2_link: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | hercules_1_port_wrist_3_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | hercules_1_rear_bumper_link: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | hercules_1_rear_left_wheel: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | hercules_1_rear_right_wheel: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | hercules_1_starboard_base: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | hercules_1_starboard_base_link: 228 | Alpha: 1 229 | Show Axes: false 230 | Show Trail: false 231 | hercules_1_starboard_base_link_inertia: 232 | Alpha: 1 233 | Show Axes: false 234 | Show Trail: false 235 | Value: true 236 | hercules_1_starboard_flange: 237 | Alpha: 1 238 | Show Axes: false 239 | Show Trail: false 240 | hercules_1_starboard_forearm_link: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | hercules_1_starboard_ft_frame: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | hercules_1_starboard_robotiq_85_base_link: 250 | Alpha: 1 251 | Show Axes: false 252 | Show Trail: false 253 | Value: true 254 | hercules_1_starboard_robotiq_85_left_finger_link: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | Value: true 259 | hercules_1_starboard_robotiq_85_left_finger_tip_link: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | hercules_1_starboard_robotiq_85_left_inner_knuckle_link: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | hercules_1_starboard_robotiq_85_left_knuckle_link: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | hercules_1_starboard_robotiq_85_right_finger_link: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | hercules_1_starboard_robotiq_85_right_finger_tip_link: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | hercules_1_starboard_robotiq_85_right_inner_knuckle_link: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | Value: true 289 | hercules_1_starboard_robotiq_85_right_knuckle_link: 290 | Alpha: 1 291 | Show Axes: false 292 | Show Trail: false 293 | Value: true 294 | hercules_1_starboard_shoulder_link: 295 | Alpha: 1 296 | Show Axes: false 297 | Show Trail: false 298 | Value: true 299 | hercules_1_starboard_tool0: 300 | Alpha: 1 301 | Show Axes: false 302 | Show Trail: false 303 | hercules_1_starboard_upper_arm_link: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | hercules_1_starboard_wrist_1_link: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | hercules_1_starboard_wrist_2_link: 314 | Alpha: 1 315 | Show Axes: false 316 | Show Trail: false 317 | Value: true 318 | hercules_1_starboard_wrist_3_link: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | hercules_1_top_chassis_link: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | Value: true 328 | hercules_1_top_plate_front_link: 329 | Alpha: 1 330 | Show Axes: false 331 | Show Trail: false 332 | hercules_1_top_plate_link: 333 | Alpha: 1 334 | Show Axes: false 335 | Show Trail: false 336 | Value: true 337 | hercules_1_top_plate_rear_link: 338 | Alpha: 1 339 | Show Axes: false 340 | Show Trail: false 341 | hercules_1_user_rail_link: 342 | Alpha: 1 343 | Show Axes: false 344 | Show Trail: false 345 | Value: true 346 | hercules_2_base_footprint: 347 | Alpha: 1 348 | Show Axes: false 349 | Show Trail: false 350 | hercules_2_base_link: 351 | Alpha: 1 352 | Show Axes: false 353 | Show Trail: false 354 | Value: true 355 | hercules_2_front_bumper_link: 356 | Alpha: 1 357 | Show Axes: false 358 | Show Trail: false 359 | Value: true 360 | hercules_2_front_left_wheel: 361 | Alpha: 1 362 | Show Axes: false 363 | Show Trail: false 364 | Value: true 365 | hercules_2_front_right_wheel: 366 | Alpha: 1 367 | Show Axes: false 368 | Show Trail: false 369 | Value: true 370 | hercules_2_gps_link: 371 | Alpha: 1 372 | Show Axes: false 373 | Show Trail: false 374 | hercules_2_imu_link: 375 | Alpha: 1 376 | Show Axes: false 377 | Show Trail: false 378 | hercules_2_inertial_link: 379 | Alpha: 1 380 | Show Axes: false 381 | Show Trail: false 382 | hercules_2_manipulator_fixture_box: 383 | Alpha: 1 384 | Show Axes: false 385 | Show Trail: false 386 | Value: true 387 | hercules_2_port_base: 388 | Alpha: 1 389 | Show Axes: false 390 | Show Trail: false 391 | hercules_2_port_base_link: 392 | Alpha: 1 393 | Show Axes: false 394 | Show Trail: false 395 | hercules_2_port_base_link_inertia: 396 | Alpha: 1 397 | Show Axes: false 398 | Show Trail: false 399 | Value: true 400 | hercules_2_port_flange: 401 | Alpha: 1 402 | Show Axes: false 403 | Show Trail: false 404 | hercules_2_port_forearm_link: 405 | Alpha: 1 406 | Show Axes: false 407 | Show Trail: false 408 | Value: true 409 | hercules_2_port_ft_frame: 410 | Alpha: 1 411 | Show Axes: false 412 | Show Trail: false 413 | hercules_2_port_robotiq_85_base_link: 414 | Alpha: 1 415 | Show Axes: false 416 | Show Trail: false 417 | Value: true 418 | hercules_2_port_robotiq_85_left_finger_link: 419 | Alpha: 1 420 | Show Axes: false 421 | Show Trail: false 422 | Value: true 423 | hercules_2_port_robotiq_85_left_finger_tip_link: 424 | Alpha: 1 425 | Show Axes: false 426 | Show Trail: false 427 | Value: true 428 | hercules_2_port_robotiq_85_left_inner_knuckle_link: 429 | Alpha: 1 430 | Show Axes: false 431 | Show Trail: false 432 | Value: true 433 | hercules_2_port_robotiq_85_left_knuckle_link: 434 | Alpha: 1 435 | Show Axes: false 436 | Show Trail: false 437 | Value: true 438 | hercules_2_port_robotiq_85_right_finger_link: 439 | Alpha: 1 440 | Show Axes: false 441 | Show Trail: false 442 | Value: true 443 | hercules_2_port_robotiq_85_right_finger_tip_link: 444 | Alpha: 1 445 | Show Axes: false 446 | Show Trail: false 447 | Value: true 448 | hercules_2_port_robotiq_85_right_inner_knuckle_link: 449 | Alpha: 1 450 | Show Axes: false 451 | Show Trail: false 452 | Value: true 453 | hercules_2_port_robotiq_85_right_knuckle_link: 454 | Alpha: 1 455 | Show Axes: false 456 | Show Trail: false 457 | Value: true 458 | hercules_2_port_shoulder_link: 459 | Alpha: 1 460 | Show Axes: false 461 | Show Trail: false 462 | Value: true 463 | hercules_2_port_tool0: 464 | Alpha: 1 465 | Show Axes: false 466 | Show Trail: false 467 | hercules_2_port_upper_arm_link: 468 | Alpha: 1 469 | Show Axes: false 470 | Show Trail: false 471 | Value: true 472 | hercules_2_port_wrist_1_link: 473 | Alpha: 1 474 | Show Axes: false 475 | Show Trail: false 476 | Value: true 477 | hercules_2_port_wrist_2_link: 478 | Alpha: 1 479 | Show Axes: false 480 | Show Trail: false 481 | Value: true 482 | hercules_2_port_wrist_3_link: 483 | Alpha: 1 484 | Show Axes: false 485 | Show Trail: false 486 | Value: true 487 | hercules_2_rear_bumper_link: 488 | Alpha: 1 489 | Show Axes: false 490 | Show Trail: false 491 | Value: true 492 | hercules_2_rear_left_wheel: 493 | Alpha: 1 494 | Show Axes: false 495 | Show Trail: false 496 | Value: true 497 | hercules_2_rear_right_wheel: 498 | Alpha: 1 499 | Show Axes: false 500 | Show Trail: false 501 | Value: true 502 | hercules_2_starboard_base: 503 | Alpha: 1 504 | Show Axes: false 505 | Show Trail: false 506 | hercules_2_starboard_base_link: 507 | Alpha: 1 508 | Show Axes: false 509 | Show Trail: false 510 | hercules_2_starboard_base_link_inertia: 511 | Alpha: 1 512 | Show Axes: false 513 | Show Trail: false 514 | Value: true 515 | hercules_2_starboard_flange: 516 | Alpha: 1 517 | Show Axes: false 518 | Show Trail: false 519 | hercules_2_starboard_forearm_link: 520 | Alpha: 1 521 | Show Axes: false 522 | Show Trail: false 523 | Value: true 524 | hercules_2_starboard_ft_frame: 525 | Alpha: 1 526 | Show Axes: false 527 | Show Trail: false 528 | hercules_2_starboard_robotiq_85_base_link: 529 | Alpha: 1 530 | Show Axes: false 531 | Show Trail: false 532 | Value: true 533 | hercules_2_starboard_robotiq_85_left_finger_link: 534 | Alpha: 1 535 | Show Axes: false 536 | Show Trail: false 537 | Value: true 538 | hercules_2_starboard_robotiq_85_left_finger_tip_link: 539 | Alpha: 1 540 | Show Axes: false 541 | Show Trail: false 542 | Value: true 543 | hercules_2_starboard_robotiq_85_left_inner_knuckle_link: 544 | Alpha: 1 545 | Show Axes: false 546 | Show Trail: false 547 | Value: true 548 | hercules_2_starboard_robotiq_85_left_knuckle_link: 549 | Alpha: 1 550 | Show Axes: false 551 | Show Trail: false 552 | Value: true 553 | hercules_2_starboard_robotiq_85_right_finger_link: 554 | Alpha: 1 555 | Show Axes: false 556 | Show Trail: false 557 | Value: true 558 | hercules_2_starboard_robotiq_85_right_finger_tip_link: 559 | Alpha: 1 560 | Show Axes: false 561 | Show Trail: false 562 | Value: true 563 | hercules_2_starboard_robotiq_85_right_inner_knuckle_link: 564 | Alpha: 1 565 | Show Axes: false 566 | Show Trail: false 567 | Value: true 568 | hercules_2_starboard_robotiq_85_right_knuckle_link: 569 | Alpha: 1 570 | Show Axes: false 571 | Show Trail: false 572 | Value: true 573 | hercules_2_starboard_shoulder_link: 574 | Alpha: 1 575 | Show Axes: false 576 | Show Trail: false 577 | Value: true 578 | hercules_2_starboard_tool0: 579 | Alpha: 1 580 | Show Axes: false 581 | Show Trail: false 582 | hercules_2_starboard_upper_arm_link: 583 | Alpha: 1 584 | Show Axes: false 585 | Show Trail: false 586 | Value: true 587 | hercules_2_starboard_wrist_1_link: 588 | Alpha: 1 589 | Show Axes: false 590 | Show Trail: false 591 | Value: true 592 | hercules_2_starboard_wrist_2_link: 593 | Alpha: 1 594 | Show Axes: false 595 | Show Trail: false 596 | Value: true 597 | hercules_2_starboard_wrist_3_link: 598 | Alpha: 1 599 | Show Axes: false 600 | Show Trail: false 601 | Value: true 602 | hercules_2_top_chassis_link: 603 | Alpha: 1 604 | Show Axes: false 605 | Show Trail: false 606 | Value: true 607 | hercules_2_top_plate_front_link: 608 | Alpha: 1 609 | Show Axes: false 610 | Show Trail: false 611 | hercules_2_top_plate_link: 612 | Alpha: 1 613 | Show Axes: false 614 | Show Trail: false 615 | Value: true 616 | hercules_2_top_plate_rear_link: 617 | Alpha: 1 618 | Show Axes: false 619 | Show Trail: false 620 | hercules_2_user_rail_link: 621 | Alpha: 1 622 | Show Axes: false 623 | Show Trail: false 624 | Value: true 625 | world: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | Mass Properties: 630 | Inertia: false 631 | Mass: false 632 | Name: RobotModel 633 | TF Prefix: "" 634 | Update Interval: 0 635 | Value: true 636 | Visual Enabled: true 637 | Enabled: true 638 | Global Options: 639 | Background Color: 48; 48; 48 640 | Fixed Frame: map 641 | Frame Rate: 30 642 | Name: root 643 | Tools: 644 | - Class: rviz_default_plugins/Interact 645 | Hide Inactive Objects: true 646 | - Class: rviz_default_plugins/MoveCamera 647 | - Class: rviz_default_plugins/Select 648 | - Class: rviz_default_plugins/FocusCamera 649 | - Class: rviz_default_plugins/Measure 650 | Line color: 128; 128; 0 651 | - Class: rviz_default_plugins/SetInitialPose 652 | Covariance x: 0.25 653 | Covariance y: 0.25 654 | Covariance yaw: 0.06853891909122467 655 | Topic: 656 | Depth: 5 657 | Durability Policy: Volatile 658 | History Policy: Keep Last 659 | Reliability Policy: Reliable 660 | Value: /initialpose 661 | - Class: rviz_default_plugins/SetGoal 662 | Topic: 663 | Depth: 5 664 | Durability Policy: Volatile 665 | History Policy: Keep Last 666 | Reliability Policy: Reliable 667 | Value: /goal_pose 668 | - Class: rviz_default_plugins/PublishPoint 669 | Single click: true 670 | Topic: 671 | Depth: 5 672 | Durability Policy: Volatile 673 | History Policy: Keep Last 674 | Reliability Policy: Reliable 675 | Value: /clicked_point 676 | Transformation: 677 | Current: 678 | Class: rviz_default_plugins/TF 679 | Value: true 680 | Views: 681 | Current: 682 | Class: rviz_default_plugins/Orbit 683 | Distance: 10.123358726501465 684 | Enable Stereo Rendering: 685 | Stereo Eye Separation: 0.05999999865889549 686 | Stereo Focal Distance: 1 687 | Swap Stereo Eyes: false 688 | Value: false 689 | Focal Point: 690 | X: -0.27755123376846313 691 | Y: 0.14179283380508423 692 | Z: -0.4846043288707733 693 | Focal Shape Fixed Size: true 694 | Focal Shape Size: 0.05000000074505806 695 | Invert Z Axis: false 696 | Name: Current View 697 | Near Clip Distance: 0.009999999776482582 698 | Pitch: 0.7103980779647827 699 | Target Frame: 700 | Value: Orbit (rviz) 701 | Yaw: 3.5835726261138916 702 | Saved: ~ 703 | Window Geometry: 704 | Displays: 705 | collapsed: false 706 | Height: 1379 707 | Hide Left Dock: false 708 | Hide Right Dock: false 709 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004a5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000049000004a5000000fc01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004a5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000049000004a5000000d101000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000050fc0100000002fb0000000800540069006d0065010000000000000a00000002bf01000003fb0000000800540069006d0065010000000000000450000000000000000000000784000004a500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 710 | Selection: 711 | collapsed: false 712 | Time: 713 | collapsed: false 714 | Tool Properties: 715 | collapsed: false 716 | Views: 717 | collapsed: false 718 | Width: 2560 719 | X: 0 720 | Y: 30 721 | -------------------------------------------------------------------------------- /hercules_description/urdf/dual_ur.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 85 | 86 | 87 | 88 | 96 | 97 | 98 | 99 | 100 | 118 | 119 | 120 | 121 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | $(arg simulation_controllers) 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | $(arg simulation_controllers) 150 | $(arg prefix)controller_manager 151 | 152 | 153 | 154 | 155 | 156 | -------------------------------------------------------------------------------- /hercules_description/urdf/hercules.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /hercules_description/urdf/hercules_macro.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 40 | 41 | 42 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 94 | 95 | 96 | 97 | 105 | 106 | 107 | 108 | 109 | 127 | 128 | 129 | 130 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | $(arg simulation_controllers) 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | $(arg simulation_controllers) 159 | ${prefix}controller_manager 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | -------------------------------------------------------------------------------- /roscon2023_control_workshop.ci.repos: -------------------------------------------------------------------------------- 1 | roscon2023_control_workshop.repos -------------------------------------------------------------------------------- /roscon2023_control_workshop.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | Universal_Robots_ROS2_Description: 3 | type: git 4 | url: https://github.com/StoglRobotics-forks/Universal_Robots_ROS2_Description 5 | version: new-gazebo-names 6 | husky: 7 | type: git 8 | url: https://github.com/StoglRobotics-forks/husky.git 9 | version: ros2control_roscon2023_workshop 10 | interactive_marker_twist_server: 11 | type: git 12 | url: https://github.com/ros-visualization/interactive_marker_twist_server.git 13 | version: humble-devel 14 | robot_upstart: 15 | type: git 16 | url: https://github.com/clearpathrobotics/robot_upstart.git 17 | version: foxy-devel 18 | robotiq_85_gripper: 19 | type: git 20 | url: https://github.com/StoglRobotics-forks/robotiq_85_gripper.git 21 | version: rolling-ros2-control-simulation 22 | ros2_control: 23 | type: git 24 | url: https://github.com/ros-controls/ros2_control.git 25 | version: master 26 | ros2_controllers: 27 | type: git 28 | url: https://github.com/ros-controls/ros2_controllers.git 29 | version: chainable-diff-drive 30 | tiago_robot: 31 | type: git 32 | url: https://github.com/bmagyar/tiago_robot.git 33 | version: roscon2023 34 | pmb2_robot: 35 | type: git 36 | url: https://github.com/pal-robotics/pmb2_robot.git 37 | version: humble-devel 38 | pal_gripper: 39 | type: git 40 | url: https://github.com/bmagyar/pal_gripper.git 41 | version: only-description -------------------------------------------------------------------------------- /roscon2023_control_workshop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(roscon2023_control_workshop) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | ament_package() 6 | -------------------------------------------------------------------------------- /roscon2023_control_workshop/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /roscon2023_control_workshop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | roscon2023_control_workshop 5 | 0.0.0 6 | Support files for the ros2_control workshop at ROSCon 2023 in New Orleans, Louisiana, USA. 7 | Dr. Denis Stogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | 13 | ament_cmake 14 | 15 | 16 | -------------------------------------------------------------------------------- /tiago_chaining/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(tiago_chaining) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | 10 | install( 11 | DIRECTORY launch config 12 | DESTINATION share/tiago_chaining 13 | ) 14 | 15 | ament_package() 16 | -------------------------------------------------------------------------------- /tiago_chaining/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | head_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | mobile_base_controller: 12 | type: diff_drive_controller/DiffDriveController 13 | 14 | cmd_vel_relay: 15 | type: twist_relay_controller/RelayController 16 | 17 | head_controller: 18 | ros__parameters: 19 | joints: 20 | - head_1_joint 21 | command_interfaces: 22 | - position 23 | state_interfaces: 24 | - position 25 | 26 | constraints: 27 | goal_time: 2.0 28 | stopped_velocity_tolerance: 5.0 29 | head_1_joint: 30 | goal: 0.5 31 | 32 | mobile_base_controller: 33 | ros__parameters: 34 | use_stamped_vel: False 35 | left_wheel_names : 36 | - wheel_left_joint 37 | right_wheel_names : 38 | - wheel_right_joint 39 | 40 | wheel_radius: 0.0985 41 | wheel_separation: 0.4044 42 | 43 | wheel_separation_multiplier: 1.0 44 | left_wheel_radius_multiplier: 1.0 45 | right_wheel_radius_multiplier: 1.0 46 | 47 | # Publish limited velocity 48 | publish_cmd: true 49 | 50 | # Publish wheel data 51 | publish_wheel_data: true 52 | 53 | cmd_vel_relay: 54 | ros__parameters: 55 | linear_velocity_cmd_if: "mobile_base_controller/angular/velocity" 56 | angular_velocity_cmd_if: "mobile_base_controller/linear/velocity" 57 | steering_angle_cmd_if: "head_controller/head_1_joint/position" 58 | yaw_multiplier: 1.0 59 | -------------------------------------------------------------------------------- /tiago_chaining/config/model_and_tf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | Splitter Ratio: 0.5 11 | Tree Height: 821 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz_default_plugins/RobotModel 53 | Collision Enabled: false 54 | Description File: "" 55 | Description Source: Topic 56 | Description Topic: 57 | Depth: 5 58 | Durability Policy: Volatile 59 | History Policy: Keep Last 60 | Reliability Policy: Reliable 61 | Value: /robot_description 62 | Enabled: true 63 | Links: 64 | All Links Enabled: true 65 | Expand Joint Details: false 66 | Expand Link Details: false 67 | Expand Tree: false 68 | Link Tree Style: Links in Alphabetic Order 69 | arm_1_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | arm_2_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | arm_3_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | arm_4_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | arm_5_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | arm_6_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | arm_7_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | arm_tool_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | base_antenna_left_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | base_antenna_right_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | base_cover_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | base_footprint: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | base_imu_link: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | base_laser_link: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | base_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | base_mic_back_left_link: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | base_mic_back_right_link: 143 | Alpha: 1 144 | Show Axes: false 145 | Show Trail: false 146 | base_mic_front_left_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | base_mic_front_right_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | base_sonar_01_link: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | base_sonar_02_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | base_sonar_03_link: 163 | Alpha: 1 164 | Show Axes: false 165 | Show Trail: false 166 | caster_back_left_1_link: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | caster_back_left_2_link: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | Value: true 175 | caster_back_right_1_link: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | caster_back_right_2_link: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | caster_front_left_1_link: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | caster_front_left_2_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | caster_front_right_1_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | caster_front_right_2_link: 198 | Alpha: 1 199 | Show Axes: false 200 | Show Trail: false 201 | Value: true 202 | gripper_grasping_frame: 203 | Alpha: 1 204 | Show Axes: false 205 | Show Trail: false 206 | gripper_left_finger_link: 207 | Alpha: 1 208 | Show Axes: false 209 | Show Trail: false 210 | Value: true 211 | gripper_link: 212 | Alpha: 1 213 | Show Axes: false 214 | Show Trail: false 215 | gripper_right_finger_link: 216 | Alpha: 1 217 | Show Axes: false 218 | Show Trail: false 219 | Value: true 220 | gripper_tool_link: 221 | Alpha: 1 222 | Show Axes: false 223 | Show Trail: false 224 | head_1_link: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | Value: true 229 | head_2_link: 230 | Alpha: 1 231 | Show Axes: false 232 | Show Trail: false 233 | Value: true 234 | head_front_camera_depth_frame: 235 | Alpha: 1 236 | Show Axes: false 237 | Show Trail: false 238 | head_front_camera_depth_optical_frame: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | head_front_camera_link: 243 | Alpha: 1 244 | Show Axes: false 245 | Show Trail: false 246 | head_front_camera_optical_frame: 247 | Alpha: 1 248 | Show Axes: false 249 | Show Trail: false 250 | head_front_camera_orbbec_aux_joint_frame: 251 | Alpha: 1 252 | Show Axes: false 253 | Show Trail: false 254 | head_front_camera_rgb_frame: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | head_front_camera_rgb_optical_frame: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | rgbd_laser_link: 263 | Alpha: 1 264 | Show Axes: false 265 | Show Trail: false 266 | torso_fixed_column_link: 267 | Alpha: 1 268 | Show Axes: false 269 | Show Trail: false 270 | Value: true 271 | torso_fixed_link: 272 | Alpha: 1 273 | Show Axes: false 274 | Show Trail: false 275 | Value: true 276 | torso_lift_link: 277 | Alpha: 1 278 | Show Axes: false 279 | Show Trail: false 280 | Value: true 281 | wheel_left_link: 282 | Alpha: 1 283 | Show Axes: false 284 | Show Trail: false 285 | Value: true 286 | wheel_right_link: 287 | Alpha: 1 288 | Show Axes: false 289 | Show Trail: false 290 | Value: true 291 | wrist_ft_link: 292 | Alpha: 1 293 | Show Axes: false 294 | Show Trail: false 295 | Value: true 296 | wrist_ft_tool_link: 297 | Alpha: 1 298 | Show Axes: false 299 | Show Trail: false 300 | Value: true 301 | Mass Properties: 302 | Inertia: false 303 | Mass: false 304 | Name: RobotModel 305 | TF Prefix: "" 306 | Update Interval: 0 307 | Value: true 308 | Visual Enabled: true 309 | - Class: rviz_default_plugins/TF 310 | Enabled: true 311 | Frame Timeout: 15 312 | Frames: 313 | All Enabled: true 314 | arm_1_link: 315 | Value: true 316 | arm_2_link: 317 | Value: true 318 | arm_3_link: 319 | Value: true 320 | arm_4_link: 321 | Value: true 322 | arm_5_link: 323 | Value: true 324 | arm_6_link: 325 | Value: true 326 | arm_7_link: 327 | Value: true 328 | arm_tool_link: 329 | Value: true 330 | base_antenna_left_link: 331 | Value: true 332 | base_antenna_right_link: 333 | Value: true 334 | base_cover_link: 335 | Value: true 336 | base_footprint: 337 | Value: true 338 | base_imu_link: 339 | Value: true 340 | base_laser_link: 341 | Value: true 342 | base_link: 343 | Value: true 344 | base_mic_back_left_link: 345 | Value: true 346 | base_mic_back_right_link: 347 | Value: true 348 | base_mic_front_left_link: 349 | Value: true 350 | base_mic_front_right_link: 351 | Value: true 352 | base_sonar_01_link: 353 | Value: true 354 | base_sonar_02_link: 355 | Value: true 356 | base_sonar_03_link: 357 | Value: true 358 | gripper_grasping_frame: 359 | Value: true 360 | gripper_left_finger_link: 361 | Value: true 362 | gripper_link: 363 | Value: true 364 | gripper_right_finger_link: 365 | Value: true 366 | gripper_tool_link: 367 | Value: true 368 | head_1_link: 369 | Value: true 370 | head_2_link: 371 | Value: true 372 | head_front_camera_depth_frame: 373 | Value: true 374 | head_front_camera_depth_optical_frame: 375 | Value: true 376 | head_front_camera_link: 377 | Value: true 378 | head_front_camera_optical_frame: 379 | Value: true 380 | head_front_camera_orbbec_aux_joint_frame: 381 | Value: true 382 | head_front_camera_rgb_frame: 383 | Value: true 384 | head_front_camera_rgb_optical_frame: 385 | Value: true 386 | odom: 387 | Value: true 388 | rgbd_laser_link: 389 | Value: true 390 | torso_fixed_column_link: 391 | Value: true 392 | torso_fixed_link: 393 | Value: true 394 | torso_lift_link: 395 | Value: true 396 | wheel_left_link: 397 | Value: true 398 | wheel_right_link: 399 | Value: true 400 | wrist_ft_link: 401 | Value: true 402 | wrist_ft_tool_link: 403 | Value: true 404 | Marker Scale: 1 405 | Name: TF 406 | Show Arrows: true 407 | Show Axes: true 408 | Show Names: false 409 | Tree: 410 | odom: 411 | base_link: 412 | base_antenna_left_link: 413 | {} 414 | base_antenna_right_link: 415 | {} 416 | base_imu_link: 417 | {} 418 | base_laser_link: 419 | {} 420 | base_mic_back_left_link: 421 | {} 422 | base_mic_back_right_link: 423 | {} 424 | base_mic_front_left_link: 425 | {} 426 | base_mic_front_right_link: 427 | {} 428 | base_sonar_01_link: 429 | {} 430 | base_sonar_02_link: 431 | {} 432 | base_sonar_03_link: 433 | {} 434 | torso_fixed_column_link: 435 | {} 436 | torso_fixed_link: 437 | torso_lift_link: 438 | arm_1_link: 439 | arm_2_link: 440 | arm_3_link: 441 | arm_4_link: 442 | arm_5_link: 443 | arm_6_link: 444 | arm_7_link: 445 | arm_tool_link: 446 | wrist_ft_link: 447 | wrist_ft_tool_link: 448 | gripper_link: 449 | gripper_grasping_frame: 450 | {} 451 | gripper_left_finger_link: 452 | {} 453 | gripper_right_finger_link: 454 | {} 455 | gripper_tool_link: 456 | {} 457 | head_1_link: 458 | head_2_link: 459 | head_front_camera_link: 460 | head_front_camera_optical_frame: 461 | {} 462 | head_front_camera_orbbec_aux_joint_frame: 463 | head_front_camera_rgb_frame: 464 | head_front_camera_depth_frame: 465 | head_front_camera_depth_optical_frame: 466 | {} 467 | head_front_camera_rgb_optical_frame: 468 | {} 469 | wheel_left_link: 470 | {} 471 | wheel_right_link: 472 | {} 473 | Update Interval: 0 474 | Value: true 475 | Enabled: true 476 | Global Options: 477 | Background Color: 48; 48; 48 478 | Fixed Frame: odom 479 | Frame Rate: 30 480 | Name: root 481 | Tools: 482 | - Class: rviz_default_plugins/Interact 483 | Hide Inactive Objects: true 484 | - Class: rviz_default_plugins/MoveCamera 485 | - Class: rviz_default_plugins/Select 486 | - Class: rviz_default_plugins/FocusCamera 487 | - Class: rviz_default_plugins/Measure 488 | Line color: 128; 128; 0 489 | - Class: rviz_default_plugins/SetInitialPose 490 | Covariance x: 0.25 491 | Covariance y: 0.25 492 | Covariance yaw: 0.06853891909122467 493 | Topic: 494 | Depth: 5 495 | Durability Policy: Volatile 496 | History Policy: Keep Last 497 | Reliability Policy: Reliable 498 | Value: /initialpose 499 | - Class: rviz_default_plugins/SetGoal 500 | Topic: 501 | Depth: 5 502 | Durability Policy: Volatile 503 | History Policy: Keep Last 504 | Reliability Policy: Reliable 505 | Value: /goal_pose 506 | - Class: rviz_default_plugins/PublishPoint 507 | Single click: true 508 | Topic: 509 | Depth: 5 510 | Durability Policy: Volatile 511 | History Policy: Keep Last 512 | Reliability Policy: Reliable 513 | Value: /clicked_point 514 | Transformation: 515 | Current: 516 | Class: rviz_default_plugins/TF 517 | Value: true 518 | Views: 519 | Current: 520 | Class: rviz_default_plugins/Orbit 521 | Distance: 6.430590629577637 522 | Enable Stereo Rendering: 523 | Stereo Eye Separation: 0.05999999865889549 524 | Stereo Focal Distance: 1 525 | Swap Stereo Eyes: false 526 | Value: false 527 | Focal Point: 528 | X: -0.13333988189697266 529 | Y: -0.23301810026168823 530 | Z: 0.4443640410900116 531 | Focal Shape Fixed Size: true 532 | Focal Shape Size: 0.05000000074505806 533 | Invert Z Axis: false 534 | Name: Current View 535 | Near Clip Distance: 0.009999999776482582 536 | Pitch: 0.38039839267730713 537 | Target Frame: 538 | Value: Orbit (rviz) 539 | Yaw: 0.4253983497619629 540 | Saved: ~ 541 | Window Geometry: 542 | Displays: 543 | collapsed: true 544 | Height: 1112 545 | Hide Left Dock: true 546 | Hide Right Dock: true 547 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000003be000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000346fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000346000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000738000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 548 | Selection: 549 | collapsed: false 550 | Time: 551 | collapsed: false 552 | Tool Properties: 553 | collapsed: false 554 | Views: 555 | collapsed: true 556 | Width: 1848 557 | X: 72 558 | Y: 51 559 | -------------------------------------------------------------------------------- /tiago_chaining/launch/key_teleop.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 18 | -------------------------------------------------------------------------------- /tiago_chaining/launch/rviz.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /tiago_chaining/launch/tiago_chaining.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /tiago_chaining/launch/tiago_show.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /tiago_chaining/launch/tiago_warmup.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /tiago_chaining/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | tiago_chaining 5 | 0.0.0 6 | Showcasing chaining controllers with the TIAGo robot 7 | Bence Magyar 8 | Bence Magyar 9 | 10 | TODO: License declaration 11 | 12 | ament_cmake 13 | launch 14 | tiago_description 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /twist_relay/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(twist_relay_controller) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | set(THIS_PACKAGE_INCLUDE_DEPENDS 9 | controller_interface 10 | generate_parameter_library 11 | geometry_msgs 12 | realtime_tools 13 | pluginlib 14 | ) 15 | 16 | find_package(ament_cmake REQUIRED) 17 | find_package(backward_ros REQUIRED) 18 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 19 | find_package(${Dependency} REQUIRED) 20 | endforeach() 21 | 22 | generate_parameter_library(twist_relay_controller_parameters 23 | src/twist_relay_controller_parameter.yaml 24 | ) 25 | 26 | add_library( 27 | twist_relay_controller 28 | SHARED 29 | src/twist_relay_controller.cpp 30 | ) 31 | target_compile_features(twist_relay_controller PUBLIC cxx_std_17) 32 | target_include_directories(twist_relay_controller PUBLIC 33 | "$" 34 | "$") 35 | target_link_libraries(twist_relay_controller PUBLIC twist_relay_controller_parameters) 36 | ament_target_dependencies(twist_relay_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) 37 | 38 | pluginlib_export_plugin_description_file( 39 | controller_interface controller_plugins.xml) 40 | 41 | ## INSTALL 42 | 43 | install( 44 | DIRECTORY include/ 45 | DESTINATION include/twist_relay_controller 46 | ) 47 | 48 | install( 49 | TARGETS twist_relay_controller twist_relay_controller_parameters 50 | EXPORT export_twist_relay_controller 51 | RUNTIME DESTINATION bin 52 | ARCHIVE DESTINATION lib 53 | LIBRARY DESTINATION lib 54 | ) 55 | 56 | ament_export_targets(export_twist_relay_controller HAS_LIBRARY_TARGET) 57 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 58 | ament_package() 59 | -------------------------------------------------------------------------------- /twist_relay/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | Controller relaying parts of a twist message 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /twist_relay/include/twist_relay/twist_relay_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, ros2_control development team 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TWIST_RELAY_CONTROLLER__TWIST_RELAY_CONTROLLER_HPP_ 16 | #define TWIST_RELAY_CONTROLLER__TWIST_RELAY_CONTROLLER_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | // auto-generated by generate_parameter_library 25 | #include "twist_relay_controller_parameters.hpp" 26 | 27 | namespace twist_relay_controller 28 | { 29 | using Twist = geometry_msgs::msg::TwistStamped; 30 | 31 | class RelayController : public controller_interface::ControllerInterface 32 | { 33 | public: 34 | RelayController(); 35 | 36 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 37 | 38 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 39 | 40 | controller_interface::return_type update( 41 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 42 | 43 | controller_interface::CallbackReturn on_init() override; 44 | 45 | controller_interface::CallbackReturn on_configure( 46 | const rclcpp_lifecycle::State & previous_state) override; 47 | 48 | protected: 49 | rclcpp::Subscription::SharedPtr twist_subscriber_ = nullptr; 50 | realtime_tools::RealtimeBox> last_msg_ptr_{nullptr}; 51 | 52 | std::shared_ptr param_listener_; 53 | Params params_; 54 | }; 55 | } // namespace twist_relay_controller 56 | 57 | #endif // TWIST_RELAY_CONTROLLER__TWIST_RELAY_CONTROLLER_HPP_ 58 | -------------------------------------------------------------------------------- /twist_relay/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | twist_relay_controller 5 | 0.0.0 6 | A wonderful controller relaying parts of a twist message 7 | Bence Magyar 8 | Apache 2 9 | 10 | ament_cmake 11 | 12 | controller_interface 13 | geometry_msgs 14 | realtime_tools 15 | pluginlib 16 | generate_parameter_library 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /twist_relay/src/twist_relay_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, ros2_control development team 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "twist_relay/twist_relay_controller.hpp" 16 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 17 | 18 | namespace 19 | { 20 | const auto DEFAULT_COMMAND_TOPIC = "/cmd_vel"; 21 | using controller_interface::interface_configuration_type; 22 | } // namespace 23 | 24 | namespace twist_relay_controller 25 | { 26 | RelayController::RelayController() : controller_interface::ControllerInterface() {} 27 | 28 | controller_interface::CallbackReturn RelayController::on_init() 29 | { 30 | try 31 | { 32 | // Create the parameter listener and get the parameters 33 | param_listener_ = std::make_shared(get_node()); 34 | params_ = param_listener_->get_params(); 35 | } 36 | catch (const std::exception & e) 37 | { 38 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 39 | return controller_interface::CallbackReturn::ERROR; 40 | } 41 | 42 | return controller_interface::CallbackReturn::SUCCESS; 43 | } 44 | 45 | controller_interface::InterfaceConfiguration RelayController::command_interface_configuration() 46 | const 47 | { 48 | std::vector conf_names = { 49 | params_.linear_velocity_cmd_if, params_.angular_velocity_cmd_if, params_.steering_angle_cmd_if}; 50 | return {interface_configuration_type::INDIVIDUAL, conf_names}; 51 | } 52 | 53 | controller_interface::InterfaceConfiguration RelayController::state_interface_configuration() const 54 | { 55 | return controller_interface::InterfaceConfiguration{ 56 | controller_interface::interface_configuration_type::NONE}; 57 | } 58 | 59 | controller_interface::CallbackReturn RelayController::on_configure( 60 | const rclcpp_lifecycle::State & /*previous_state*/) 61 | { 62 | params_ = param_listener_->get_params(); 63 | RCLCPP_INFO(get_node()->get_logger(), "Parameters were updated"); 64 | 65 | twist_subscriber_ = get_node()->create_subscription( 66 | DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), 67 | [this](const std::shared_ptr msg) -> void 68 | { 69 | if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) 70 | { 71 | msg->header.stamp = get_node()->get_clock()->now(); 72 | } 73 | last_msg_ptr_.set(std::move(msg)); 74 | }); 75 | 76 | return controller_interface::CallbackReturn::SUCCESS; 77 | } 78 | 79 | controller_interface::return_type RelayController::update( 80 | const rclcpp::Time & time, const rclcpp::Duration & /*period*/) 81 | { 82 | std::shared_ptr last_command_msg; 83 | last_msg_ptr_.get(last_command_msg); 84 | if (last_command_msg != nullptr) 85 | { 86 | RCLCPP_INFO(get_node()->get_logger(), "UPDATED"); 87 | 88 | command_interfaces_[0].set_value(last_command_msg->twist.linear.x); 89 | command_interfaces_[1].set_value(last_command_msg->twist.angular.z); 90 | double fake_steering_angle = last_command_msg->twist.angular.z * params_.yaw_multiplier; 91 | command_interfaces_[2].set_value(fake_steering_angle); 92 | } 93 | return controller_interface::return_type::OK; 94 | } 95 | 96 | } // namespace twist_relay_controller 97 | 98 | #include "pluginlib/class_list_macros.hpp" 99 | 100 | PLUGINLIB_EXPORT_CLASS( 101 | twist_relay_controller::RelayController, controller_interface::ControllerInterface) 102 | -------------------------------------------------------------------------------- /twist_relay/src/twist_relay_controller_parameter.yaml: -------------------------------------------------------------------------------- 1 | twist_relay_controller: 2 | linear_velocity_cmd_if: { 3 | type: string, 4 | description: "", 5 | } 6 | angular_velocity_cmd_if: { 7 | type: string, 8 | description: "", 9 | } 10 | steering_angle_cmd_if: { 11 | type: string, 12 | description: "", 13 | } 14 | yaw_multiplier: { 15 | type: double, 16 | default_value: 1.0, 17 | } 18 | --------------------------------------------------------------------------------