├── .clang-format ├── .clang-tidy ├── .github └── workflows │ ├── bloom-release.yml │ ├── deb_package.yml │ ├── doxygen.yml │ ├── format.yml │ └── industrial_ci.yml ├── .gitignore ├── .pre-commit-config.yaml ├── Doxyfile ├── LICENSE ├── README.md ├── gpio_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── gpio_controller_plugins.xml ├── include │ └── gpio_controller │ │ └── gpio_controller.h ├── package.xml ├── src │ └── gpio_controller.cpp └── test │ ├── gpio_controller.launch │ └── gpio_controller.yaml ├── mimic_joint_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mimic_joint_controller │ │ └── mimic_joint_controller.h ├── mimic_joint_controller_plugins.xml ├── package.xml └── src │ └── mimic_joint_controller.cpp ├── rm_calibration_controllers ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── rm_calibration_controllers │ │ ├── calibration_base.h │ │ ├── differential_calibration_controller.h │ │ ├── gpio_calibration_controller.h │ │ └── mechanical_calibration_controller.h ├── package.xml ├── rm_calibration_controllers_plugins.xml └── src │ ├── calibration_base.cpp │ ├── differential_calibration_controller.cpp │ ├── gpio_calibration_controller.cpp │ └── mechanical_calibration_controller.cpp ├── rm_chassis_controllers ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── rm_chassis_controllers │ │ ├── balance.h │ │ ├── chassis_base.h │ │ ├── omni.h │ │ ├── sentry.h │ │ └── swerve.h ├── package.xml ├── rm_chassis_controllers_plugins.xml ├── src │ ├── balance.cpp │ ├── chassis_base.cpp │ ├── omni.cpp │ ├── sentry.cpp │ └── swerve.cpp └── test │ ├── balance.yaml │ ├── load_controllers.launch │ ├── mecanum.yaml │ ├── omni.yaml │ └── swerve.yaml ├── rm_controllers ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── rm_gimbal_controllers ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ ├── BulletSolver.cfg │ └── GimbalBase.cfg ├── include │ └── rm_gimbal_controllers │ │ ├── bullet_solver.h │ │ └── gimbal_base.h ├── package.xml ├── rm_gimbal_controllers_plugins.xml ├── src │ ├── bullet_solver.cpp │ └── gimbal_base.cpp └── test │ ├── gimbal_config_template.yaml │ └── load_controllers.launch ├── rm_orientation_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── rm_orientation_controller │ │ └── orientation_controller.h ├── package.xml ├── rm_orientation_controller_plugin.xml ├── src │ └── orientation_controller.cpp └── test │ ├── load_controllers.launch │ └── orientation_config_template.yaml ├── rm_shooter_controllers ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ └── Shooter.cfg ├── doc │ └── example.jpg ├── include │ └── rm_shooter_controllers │ │ └── standard.h ├── package.xml ├── rm_shooter_controllers_plugins.xml ├── src │ └── standard.cpp └── test │ ├── load_controllers.launch │ └── shooter_config_template.yaml ├── robot_state_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── robot_state_controller │ │ └── robot_state_controller.h ├── package.xml ├── robot_state_controller_plugin.xml ├── src │ └── robot_state_controller.cpp └── test │ ├── config.yaml │ └── load_controllers.launch └── tof_radar_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── tof_radar_controller │ └── tof_radar_controller.h ├── package.xml ├── src └── tof_radar_controller.cpp ├── test ├── load_controller.launch └── tof_config_template.yaml └── tof_radar_controller_plugins.xml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | MaxEmptyLinesToKeep: 1 5 | SortIncludes: false 6 | 7 | Standard: Auto 8 | IndentWidth: 2 9 | TabWidth: 2 10 | UseTab: Never 11 | AccessModifierOffset: -2 12 | ConstructorInitializerIndentWidth: 2 13 | NamespaceIndentation: None 14 | ContinuationIndentWidth: 4 15 | IndentCaseLabels: true 16 | IndentFunctionDeclarationAfterType: false 17 | 18 | AlignEscapedNewlinesLeft: false 19 | AlignTrailingComments: true 20 | 21 | AllowAllParametersOfDeclarationOnNextLine: false 22 | ExperimentalAutoDetectBinPacking: false 23 | ObjCSpaceBeforeProtocolList: true 24 | Cpp11BracedListStyle: false 25 | 26 | AllowShortBlocksOnASingleLine: true 27 | AllowShortIfStatementsOnASingleLine: false 28 | AllowShortLoopsOnASingleLine: false 29 | AllowShortFunctionsOnASingleLine: None 30 | AllowShortCaseLabelsOnASingleLine: false 31 | 32 | AlwaysBreakTemplateDeclarations: true 33 | AlwaysBreakBeforeMultilineStrings: false 34 | BreakBeforeBinaryOperators: false 35 | BreakBeforeTernaryOperators: false 36 | BreakConstructorInitializersBeforeComma: true 37 | 38 | BinPackParameters: true 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | DerivePointerBinding: false 41 | PointerBindsToType: true 42 | 43 | PenaltyExcessCharacter: 50 44 | PenaltyBreakBeforeFirstCallParameter: 30 45 | PenaltyBreakComment: 1000 46 | PenaltyBreakFirstLessLess: 10 47 | PenaltyBreakString: 100 48 | PenaltyReturnTypeOnItsOwnLine: 50 49 | 50 | SpacesBeforeTrailingComments: 2 51 | SpacesInParentheses: false 52 | SpacesInAngles: false 53 | SpaceInEmptyParentheses: false 54 | SpacesInCStyleCastParentheses: false 55 | SpaceAfterCStyleCast: false 56 | SpaceAfterControlStatementKeyword: true 57 | SpaceBeforeAssignmentOperators: true 58 | 59 | # Configure each individual brace in BraceWrapping 60 | BreakBeforeBraces: Custom 61 | 62 | # Control of individual brace wrapping cases 63 | BraceWrapping: 64 | AfterCaseLabel: true 65 | AfterClass: true 66 | AfterControlStatement: true 67 | AfterEnum: true 68 | AfterFunction: true 69 | AfterNamespace: true 70 | AfterStruct: true 71 | AfterUnion: true 72 | BeforeCatch: true 73 | BeforeElse: true 74 | IndentBraces: false 75 | ... 76 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: '-*, 3 | performance-*, 4 | llvm-namespace-comment, 5 | modernize-redundant-void-arg, 6 | modernize-use-nullptr, 7 | modernize-use-default, 8 | modernize-use-override, 9 | modernize-loop-convert, 10 | modernize-make-shared, 11 | modernize-make-unique, 12 | readability-named-parameter, 13 | readability-redundant-smartptr-get, 14 | readability-redundant-string-cstr, 15 | readability-simplify-boolean-expr, 16 | readability-container-size-empty, 17 | readability-identifier-naming, 18 | ' 19 | HeaderFilterRegex: '' 20 | AnalyzeTemporaryDtors: false 21 | CheckOptions: 22 | - key: llvm-namespace-comment.ShortNamespaceLines 23 | value: '10' 24 | - key: llvm-namespace-comment.SpacesBeforeComments 25 | value: '2' 26 | - key: readability-braces-around-statements.ShortStatementLines 27 | value: '2' 28 | # type names 29 | - key: readability-identifier-naming.ClassCase 30 | value: CamelCase 31 | - key: readability-identifier-naming.EnumCase 32 | value: CamelCase 33 | - key: readability-identifier-naming.UnionCase 34 | value: CamelCase 35 | # method names 36 | - key: readability-identifier-naming.MethodCase 37 | value: camelBack 38 | # variable names 39 | - key: readability-identifier-naming.VariableCase 40 | value: lower_case 41 | - key: readability-identifier-naming.ClassMemberSuffix 42 | value: '_' 43 | # const static or global variables are UPPER_CASE 44 | - key: readability-identifier-naming.EnumConstantCase 45 | value: UPPER_CASE 46 | - key: readability-identifier-naming.StaticConstantCase 47 | value: UPPER_CASE 48 | - key: readability-identifier-naming.ClassConstantCase 49 | value: UPPER_CASE 50 | - key: readability-identifier-naming.GlobalVariableCase 51 | value: UPPER_CASE 52 | ... 53 | -------------------------------------------------------------------------------- /.github/workflows/bloom-release.yml: -------------------------------------------------------------------------------- 1 | name: bloom-release 2 | on: 3 | push: 4 | paths: 5 | - package.xml 6 | - '*/package.xml' 7 | branches: 8 | - master 9 | workflow_dispatch: 10 | 11 | jobs: 12 | bloom-release: 13 | runs-on: ubuntu-latest 14 | steps: 15 | - name: checkout 16 | uses: actions/checkout@v2 17 | - name: bloom release 18 | id: bloom 19 | uses: at-wat/bloom-release-action@v0 20 | with: 21 | ros_distro: noetic 22 | github_token_bloom: ${{ secrets.BLOOM_GITHUB_TOKEN }} 23 | github_user: qiayuanliao 24 | git_user: qiayuan 25 | git_email: liaoqiayuan@gmail.com 26 | release_repository_push_url: https://github.com/${{ github.repository }}-release.git 27 | tag_and_release: true 28 | open_pr: true 29 | debug_bloom: true 30 | - name: create GitHub release 31 | uses: actions/create-release@v1 32 | continue-on-error: true 33 | env: 34 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 35 | with: 36 | tag_name: ${{ steps.bloom.outputs.version }} 37 | release_name: Release ${{ steps.bloom.outputs.version }} 38 | -------------------------------------------------------------------------------- /.github/workflows/deb_package.yml: -------------------------------------------------------------------------------- 1 | name: deb-package 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | branches: 8 | - master 9 | workflow_dispatch: 10 | jobs: 11 | build: 12 | runs-on: ubuntu-20.04 13 | steps: 14 | - uses: actions/checkout@v2 15 | - uses: actions/checkout@v2 16 | with: 17 | repository: rm-controls/rm_control 18 | path: 'rm_control' 19 | - uses: ros-tooling/setup-ros@v0.2 20 | with: 21 | required-ros-distributions: noetic 22 | - name: Package deb 23 | uses: gdut-dynamic-x/ros-build-deb-action@v1 24 | with: 25 | ros_distro: noetic 26 | timestamp: true 27 | - name: Upload deb package to source 28 | if: ${{ github.repository == 'rm-controls/rm_controllers' }} 29 | uses: appleboy/scp-action@master 30 | with: 31 | host: ${{ secrets.SOURCE_SSH_HOST }} 32 | username: ${{ secrets.SOURCE_SSH_NAME }} 33 | key: ${{ secrets.SOURCE_SSH_KEYGEN }} 34 | port: ${{ secrets.SOURCE_SSH_PORT }} 35 | source: "*.deb" 36 | target: "/home/dynamicx/package_hub/wwwroot/ppa/incoming" 37 | - name: Deploy deb package to source 38 | if: ${{ github.repository == 'rm-controls/rm_controllers' }} 39 | uses: appleboy/ssh-action@master 40 | with: 41 | host: ${{ secrets.SOURCE_SSH_HOST }} 42 | username: ${{ secrets.SOURCE_SSH_NAME }} 43 | key: ${{ secrets.SOURCE_SSH_KEYGEN }} 44 | port: ${{ secrets.SOURCE_SSH_PORT }} 45 | script: | 46 | cd /home/dynamicx/package_hub/wwwroot/ppa/ 47 | bash deploy.sh 48 | -------------------------------------------------------------------------------- /.github/workflows/doxygen.yml: -------------------------------------------------------------------------------- 1 | name: Doxygen-docs 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request_target: 7 | branches: [ master ] 8 | types: [opened, synchronize, reopened] 9 | 10 | jobs: 11 | docs: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v2 15 | - run: mkdir build 16 | - uses: mattnotmitt/doxygen-action@v1 17 | 18 | - name: Get Branch Name 19 | run: echo "BRANCH_NAME=$(echo ${GITHUB_REF#refs/heads/})" >> $GITHUB_ENV 20 | 21 | - name: Get Deploy Name 22 | if: github.event_name == 'push' 23 | run: | 24 | echo "DEPLOY_NAME=${{ env.BRANCH_NAME }}" >> $GITHUB_ENV 25 | echo "PRODUCTION=${{ env.BRANCH_NAME == 'master' }}" >> $GITHUB_ENV 26 | 27 | - name: Get Deploy Name 28 | if: github.event_name != 'push' 29 | run: | 30 | echo "DEPLOY_NAME=deploy-preview-${{ github.event.number }}" >> $GITHUB_ENV 31 | echo "PRODUCTION=false" >> $GITHUB_ENV 32 | 33 | - name: Deploy to Netlify 34 | uses: nwtgck/actions-netlify@v1.1 35 | with: 36 | publish-dir: './build/docs/html' 37 | production-deploy: ${{ env.PRODUCTION }} 38 | github-token: ${{ secrets.GITHUB_TOKEN }} 39 | deploy-message: 'Deploy ${{ env.DEPLOY_NAME }}@${{ github.sha }}' 40 | enable-pull-request-comment: false 41 | enable-commit-comment: false 42 | alias: ${{ env.DEPLOY_NAME }} 43 | env: 44 | NETLIFY_AUTH_TOKEN: ${{ secrets.NETLIFY_AUTH_TOKEN }} 45 | NETLIFY_SITE_ID: ${{ secrets.NETLIFY_SITE_ID }} 46 | -------------------------------------------------------------------------------- /.github/workflows/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 | push: 10 | 11 | jobs: 12 | pre-commit: 13 | name: pre-commit 14 | runs-on: ubuntu-22.04 15 | steps: 16 | - uses: actions/checkout@v3 17 | - uses: actions/setup-python@v3 18 | - name: Install clang-format 19 | run: | 20 | sudo apt update 21 | sudo apt install clang-format 22 | - uses: pre-commit/action@v3.0.0 23 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [ push, pull_request ] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - { ROS_DISTRO: noetic, ROS_REPO: testing } 11 | - { ROS_DISTRO: noetic, ROS_REPO: main } 12 | - { ROS_DISTRO: melodic, ROS_REPO: testing } 13 | - { ROS_DISTRO: melodic, ROS_REPO: main } 14 | env: 15 | CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) 16 | UPSTREAM_WORKSPACE: 'github:rm-controls/rm_control#master github:rm-controls/rm_description#master' 17 | runs-on: ubuntu-latest 18 | steps: 19 | - uses: actions/checkout@v2 20 | # This step will fetch/store the directory used by ccache before/after the ci run 21 | - uses: actions/cache@v2 22 | with: 23 | path: ${{ env.CCACHE_DIR }} 24 | key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} 25 | # Run industrial_ci 26 | - uses: 'ros-industrial/industrial_ci@master' 27 | env: ${{ matrix.env }} 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | # Doxygen 54 | docs/ 55 | -------------------------------------------------------------------------------- /.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 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks.git 18 | rev: v4.0.1 19 | hooks: 20 | - id: check-added-large-files 21 | - id: check-case-conflict 22 | - id: check-merge-conflict 23 | - id: check-symlinks 24 | - id: check-yaml 25 | - id: debug-statements 26 | - id: end-of-file-fixer 27 | - id: mixed-line-ending 28 | - id: trailing-whitespace 29 | 30 | - repo: https://github.com/psf/black 31 | rev: 21.9b0 32 | hooks: 33 | - id: black 34 | 35 | - repo: https://github.com/Lucas-C/pre-commit-hooks-markup 36 | rev: v1.0.1 37 | hooks: 38 | - id: rst-linter 39 | exclude: .*/doc/.* 40 | 41 | - repo: local 42 | hooks: 43 | - id: clang-format 44 | name: clang-format 45 | description: Format files with ClangFormat. 46 | entry: clang-format 47 | language: system 48 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 49 | args: [ "-fallback-style=none", "-i" ] 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Qiayuan Liao 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rm_controllers 2 | ## Status 3 | ### Continuous Integration 4 | [![CI](https://github.com/rm-controls/rm_controllers/actions/workflows/industrial_ci.yml/badge.svg)](https://github.com/rm-controls/rm_controllers/actions/workflows/industrial_ci.yml) 5 | [![Format](https://github.com/rm-controls/rm_controllers/actions/workflows/format.yml/badge.svg)](https://github.com/rm-controls/rm_controllers/actions/workflows/format.yml) 6 | [![Doxygen-docs](https://github.com/rm-controls/rm_controllers/actions/workflows/doxygen.yml/badge.svg)](https://github.com/rm-controls/rm_controllers/actions/workflows/doxygen.yml) 7 | 8 | ### Licenses 9 | [![FOSSA Status](https://app.fossa.com/api/projects/git%2Bgithub.com%2Frm-controls%2Frm_controllers.svg?type=large)](https://app.fossa.com/projects/git%2Bgithub.com%2Frm-controls%2Frm_controllers?ref=badge_large) 10 | ## Documentation 11 | [rm-controls Documentation](https://rm-control-docs.netlify.app/) 12 | 13 | [rm-control Doxygen Documentation](https://rm-controller.netlify.app/) 14 | -------------------------------------------------------------------------------- /gpio_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package gpio_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 29 | 0.1.7 30 | * Contributors: ye-luo-xi-tui 31 | 32 | 0.1.7 (2022-09-10) 33 | ------------------ 34 | * Merge remote-tracking branch 'origin/master' 35 | * Contributors: qiayuan 36 | 37 | 0.1.6 (2022-06-16) 38 | ------------------ 39 | * Merge remote-tracking branch 'origin/master' 40 | * Contributors: qiayuan 41 | 42 | 0.1.5 (2022-06-10) 43 | ------------------ 44 | * Merge remote-tracking branch 'origin/master' 45 | * Merge pull request `#70 `_ from XYM-github/gpio_controller_2 46 | Update gpio_controller to version2.0. 47 | * Optimize code structure. 48 | * Modify gpio_controller configuration file format. 49 | * Adaptation of gpio type moved from rm_hw to rm_common. 50 | * Adaptation of gpio type to enum in gpio_interface. 51 | * Update gpio_controller to version2.0. 52 | * Contributors: QiayuanLiao, XYM-github, yezi 53 | -------------------------------------------------------------------------------- /gpio_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(gpio_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | 7 | rm_common 8 | 9 | pluginlib 10 | controller_interface 11 | realtime_tools 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS 16 | include 17 | CATKIN_DEPENDS 18 | roscpp 19 | 20 | rm_common 21 | 22 | pluginlib 23 | controller_interface 24 | realtime_tools 25 | LIBRARIES ${PROJECT_NAME} 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | add_library(${PROJECT_NAME} 34 | src/gpio_controller.cpp 35 | ) 36 | 37 | target_link_libraries(${PROJECT_NAME} 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | install( 42 | TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | 48 | # Mark cpp header files for installation 49 | install( 50 | DIRECTORY include/${PROJECT_NAME}/ 51 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 52 | FILES_MATCHING PATTERN "*.h" 53 | ) 54 | 55 | # Mark other files for installation 56 | install( 57 | DIRECTORY test 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | install( 61 | FILES gpio_controller_plugins.xml 62 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 63 | ) 64 | -------------------------------------------------------------------------------- /gpio_controller/gpio_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /gpio_controller/include/gpio_controller/gpio_controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/4/29. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace gpio_controller 14 | { 15 | class Controller : public controller_interface::MultiInterfaceController 17 | { 18 | public: 19 | Controller() = default; 20 | 21 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 22 | 23 | void update(const ros::Time& time, const ros::Duration& period) override; 24 | 25 | private: 26 | void setGpioCmd(const rm_msgs::GpioDataConstPtr& msg); 27 | 28 | std::vector gpio_state_handles_; 29 | std::vector gpio_command_handles_; 30 | 31 | ros::Subscriber cmd_subscriber_; 32 | typedef std::shared_ptr> RtpublisherPtr; 33 | RtpublisherPtr gpio_state_pub_; 34 | }; 35 | } // namespace gpio_controller 36 | -------------------------------------------------------------------------------- /gpio_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gpio_controller 4 | 0.1.11 5 | The gpio_controller package 6 | muyuexin 7 | 8 | BSD 9 | 10 | catkin 11 | 12 | rm_common 13 | pluginlib 14 | roscpp 15 | controller_interface 16 | realtime_tools 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /gpio_controller/src/gpio_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by muyuexin on 2022/4/29. 3 | // 4 | 5 | #include 6 | #include "gpio_controller/gpio_controller.h" 7 | #include 8 | 9 | namespace gpio_controller 10 | { 11 | bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) 12 | { 13 | XmlRpc::XmlRpcValue xml_rpc_value; 14 | controller_nh.getParam("gpios", xml_rpc_value); 15 | ROS_ASSERT(xml_rpc_value.getType() == XmlRpc::XmlRpcValue::TypeArray); 16 | 17 | // realtime publisher 18 | gpio_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "gpio_states", 100)); 19 | 20 | for (int i = 0; i < xml_rpc_value.size(); ++i) 21 | { 22 | ROS_ASSERT(xml_rpc_value[i].getType() == XmlRpc::XmlRpcValue::TypeString); 23 | std::string gpioName = xml_rpc_value[i]; 24 | rm_control::GpioStateHandle state_handle_ = robot_hw->get()->getHandle(gpioName); 25 | gpio_state_handles_.push_back(state_handle_); 26 | gpio_state_pub_->msg_.gpio_name.push_back(state_handle_.getName()); 27 | gpio_state_pub_->msg_.gpio_state.push_back(state_handle_.getValue()); 28 | if (state_handle_.getType() == rm_control::OUTPUT) 29 | { 30 | gpio_state_pub_->msg_.gpio_type.push_back("out"); 31 | } 32 | else 33 | { 34 | gpio_state_pub_->msg_.gpio_type.push_back("in"); 35 | } 36 | ROS_INFO("Got state_gpio %s", gpioName.c_str()); 37 | if (state_handle_.getType() == rm_control::OUTPUT) 38 | { 39 | rm_control::GpioCommandHandle command_handle_ = 40 | robot_hw->get()->getHandle(gpioName); 41 | gpio_command_handles_.push_back(command_handle_); 42 | ROS_INFO("Got command_gpio %s", gpioName.c_str()); 43 | } 44 | } 45 | 46 | cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::setGpioCmd, this); 47 | return true; 48 | } 49 | 50 | void Controller::update(const ros::Time& time, const ros::Duration& period) 51 | { 52 | if (gpio_state_pub_->trylock()) 53 | { 54 | for (unsigned i = 0; i < gpio_state_handles_.size(); i++) 55 | { 56 | gpio_state_pub_->msg_.gpio_state[i] = gpio_state_handles_[i].getValue(); 57 | } 58 | gpio_state_pub_->msg_.header.stamp = time; 59 | gpio_state_pub_->unlockAndPublish(); 60 | } 61 | } 62 | 63 | void Controller::setGpioCmd(const rm_msgs::GpioDataConstPtr& msg) 64 | { 65 | for (unsigned i = 0; i < gpio_command_handles_.size(); i++) 66 | { 67 | for (unsigned j = 0; j < msg->gpio_name.size(); j++) 68 | { 69 | if ((msg->gpio_name[j].find(gpio_command_handles_[i].getName()) != std::string::npos)) 70 | { 71 | gpio_command_handles_[i].setCommand(msg->gpio_state[j]); 72 | } 73 | } 74 | } 75 | return; 76 | } 77 | 78 | } // namespace gpio_controller 79 | 80 | PLUGINLIB_EXPORT_CLASS(gpio_controller::Controller, controller_interface::ControllerBase) 81 | -------------------------------------------------------------------------------- /gpio_controller/test/gpio_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /gpio_controller/test/gpio_controller.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | gpio_controller: 3 | type: gpio_controller/Controller 4 | gpios: [ "gripper1", "gripper2", "gripper3" ] 5 | -------------------------------------------------------------------------------- /mimic_joint_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mimic_joint_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 29 | 0.1.7 30 | * Contributors: ye-luo-xi-tui 31 | 32 | 0.1.7 (2022-09-10) 33 | ------------------ 34 | * Merge remote-tracking branch 'origin/master' 35 | * Contributors: qiayuan 36 | 37 | 0.1.6 (2022-06-16) 38 | ------------------ 39 | * Merge remote-tracking branch 'origin/master' 40 | * Contributors: qiayuan 41 | 42 | 0.1.5 (2022-06-10) 43 | ------------------ 44 | * Merge remote-tracking branch 'origin/master' 45 | * Merge pull request `#67 `_ from ljq-lv/joint_mime_controller 46 | Add Joint mime controller 47 | * Modified name "mimic_joint" to "target_joint" 48 | * Modified "joint_mime" to "mimic_joint" 49 | * Modified the name from "mime" to "mimic" 50 | * Contributors: QiayuanLiao, chenzheng, ljq-lv 51 | -------------------------------------------------------------------------------- /mimic_joint_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mimic_joint_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | 7 | pluginlib 8 | controller_interface 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS 13 | include 14 | CATKIN_DEPENDS 15 | roscpp 16 | 17 | pluginlib 18 | controller_interface 19 | LIBRARIES ${PROJECT_NAME} 20 | ) 21 | 22 | include_directories( 23 | include 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | add_library(${PROJECT_NAME} 28 | src/mimic_joint_controller.cpp 29 | ) 30 | 31 | target_link_libraries(${PROJECT_NAME} 32 | ${catkin_LIBRARIES} 33 | ) 34 | 35 | install( 36 | TARGETS ${PROJECT_NAME} 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | 42 | # Mark cpp header files for installation 43 | install( 44 | DIRECTORY include/${PROJECT_NAME}/ 45 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 46 | FILES_MATCHING PATTERN "*.h" 47 | ) 48 | 49 | # Mark other files for installation 50 | install( 51 | FILES mimic_joint_controller_plugins.xml 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 53 | ) 54 | -------------------------------------------------------------------------------- /mimic_joint_controller/include/mimic_joint_controller/mimic_joint_controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ljq on 2022/5/15. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace mimic_joint_controller 14 | { 15 | class MimicJointController 16 | : public controller_interface::MultiInterfaceController 18 | { 19 | public: 20 | MimicJointController() = default; 21 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) override; 22 | void update(const ros::Time& time, const ros::Duration& period) override; 23 | 24 | private: 25 | effort_controllers::JointPositionController mimic_joint_ctrl_; 26 | hardware_interface::JointStateHandle target_state_handle_; 27 | std::string target_joint_name_; 28 | }; 29 | 30 | } // namespace mimic_joint_controller 31 | 32 | PLUGINLIB_EXPORT_CLASS(mimic_joint_controller::MimicJointController, controller_interface::ControllerBase) 33 | -------------------------------------------------------------------------------- /mimic_joint_controller/mimic_joint_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mimic_joint_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mimic_joint_controller 4 | 0.1.11 5 | The mimic_joint_controller package 6 | ljq 7 | 8 | BSD 9 | 10 | catkin 11 | 12 | pluginlib 13 | roscpp 14 | controller_interface 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /mimic_joint_controller/src/mimic_joint_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ljq on 2022/5/15. 3 | // 4 | 5 | #include "mimic_joint_controller/mimic_joint_controller.h" 6 | 7 | namespace mimic_joint_controller 8 | { 9 | bool MimicJointController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) 10 | { 11 | if (!controller_nh.getParam("target_joint_name", target_joint_name_)) 12 | { 13 | ROS_ERROR("mimic_joint is not set"); 14 | return false; 15 | } 16 | target_state_handle_ = robot_hw->get()->getHandle(target_joint_name_); 17 | mimic_joint_ctrl_.init(robot_hw->get(), controller_nh); 18 | return true; 19 | }; 20 | 21 | void MimicJointController::update(const ros::Time& time, const ros::Duration& period) 22 | { 23 | mimic_joint_ctrl_.setCommand(target_state_handle_.getPosition()); 24 | mimic_joint_ctrl_.update(time, period); 25 | }; 26 | 27 | } // namespace mimic_joint_controller 28 | -------------------------------------------------------------------------------- /rm_calibration_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rm_calibration_controllers) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | roscpp 16 | 17 | rm_common 18 | controller_interface 19 | effort_controllers 20 | ) 21 | 22 | ################################### 23 | ## catkin specific configuration ## 24 | ################################### 25 | ## The catkin_package macro generates cmake config files for your package 26 | ## Declare things to be passed to dependent projects 27 | ## INCLUDE_DIRS: uncomment this if your package contains header files 28 | ## LIBRARIES: libraries you create in this project that dependent projects also need 29 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 30 | ## DEPENDS: system dependencies of this project that dependent projects also need 31 | catkin_package( 32 | INCLUDE_DIRS 33 | include 34 | LIBRARIES 35 | CATKIN_DEPENDS 36 | roscpp 37 | 38 | rm_common 39 | effort_controllers 40 | LIBRARIES ${PROJECT_NAME} 41 | ) 42 | 43 | ########### 44 | ## Build ## 45 | ########### 46 | 47 | ## Specify additional locations of header files 48 | include_directories( 49 | include 50 | ${catkin_INCLUDE_DIRS} 51 | ) 52 | 53 | ## Declare a cpp library 54 | add_library(${PROJECT_NAME} 55 | src/calibration_base.cpp 56 | src/gpio_calibration_controller.cpp 57 | src/mechanical_calibration_controller.cpp 58 | src/differential_calibration_controller.cpp 59 | ) 60 | 61 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 62 | 63 | ############# 64 | ## Install ## 65 | ############# 66 | 67 | ## Mark executables and/or libraries for installation 68 | install( 69 | TARGETS ${PROJECT_NAME} 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | # Mark cpp header files for installation 76 | install( 77 | DIRECTORY include/${PROJECT_NAME}/ 78 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 79 | FILES_MATCHING PATTERN "*.h" 80 | ) 81 | 82 | # Mark other files for installation 83 | install( 84 | FILES rm_calibration_controllers_plugins.xml 85 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 86 | ) 87 | 88 | ############# 89 | ## Testing ## 90 | ############# 91 | 92 | #if (${CATKIN_ENABLE_TESTING}) 93 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 94 | # ## Add gtest based cpp test target and link libraries 95 | # catkin_add_gtest(${PROJECT_NAME}-test 96 | # test/test_ros_package_template.cpp 97 | # test/AlgorithmTest.cpp) 98 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 99 | #endif () 100 | -------------------------------------------------------------------------------- /rm_calibration_controllers/README.md: -------------------------------------------------------------------------------- 1 | # rm_calibration_controllers 2 | 3 | ## Overview 4 | 5 | Since the zero point of some motors will change after power off, rm_calibration_controllers will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stops moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed until gpio changes to be different from initial gpio.Then it will retreat at a small angle and restore initial gpio.Finally it moves at a slow speed until the state of gpio changes to be different from initial gpio again. 6 | 7 | **Keywords:** calibration, ROS, position. 8 | 9 | ### License 10 | 11 | The source code is released under a [ BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/rm_calibration_controllers/LICENSE). 12 | 13 | **Author: DynamicX
14 | Affiliation: DynamicX
15 | Maintainer: DynamicX** 16 | 17 | The package has been tested under [ROS](https://www.ros.org/) Indigo, Melodic and Noetic on respectively Ubuntu 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 18 | 19 | ### Hardware interface type 20 | 21 | + `EffortJointInterface` Used to send effort command to target joint to make it reach the calibration speed. 22 | + `ActuatorExtraInterface` Used to obtain the information of the target actuators offset, current position, the state of the whether it is stopped and the state of whether it is calibrated. 23 | + `GpioStateInterface` Used to obtain the state of gpio. 24 | 25 | 26 | ## Installation 27 | 28 | ### Installation from Packages 29 | 30 | To install all packages from the this repository as Debian packages use 31 | 32 | sudo apt-get install ros-noetic-calibration-controllers 33 | 34 | Or better, use `rosdep`: 35 | 36 | sudo rosdep install --from-paths src 37 | 38 | ### Dependencies 39 | 40 | * roscpp 41 | * rm_common 42 | * effort_controllers 43 | * controller_interface 44 | 45 | ## ROS API 46 | 47 | ### Service 48 | 49 | * **`is_calibrated`** ([control_msgs/QueryCalibrationState](http://docs.ros.org/en/api/control_msgs/html/srv/QueryCalibrationState.html)) 50 | 51 | When requesting to this server, it will return response about whether target joint has been calibrated. 52 | 53 | 54 | ### Parameters 55 | 56 | #### calibration_base 57 | 58 | * **`search_velocity`** (double) 59 | 60 | The joint velocity of calibrating. 61 | 62 | #### mechanical_calibration_controller 63 | 64 | * **`velocity_threshold_`** (double) 65 | 66 | This is velocity `threshold`. When the real time velocity of target joint lower than threshold, and last for a while, 67 | it can switch CALIBRATED from MOVING. 68 | 69 | #### gpio_calibration_controller 70 | 71 | * **`backward_angle`** (double) 72 | 73 | The angle of retreat when gpio changes to be different form initial gpio for the first time. 74 | 75 | * **`slow_forward_velocity`** (double) 76 | 77 | The velocity of second forward movement for reaching a more accurate calibration-position. 78 | 79 | * **`pos_threshold`** (double) 80 | 81 | The threshold for the difference between the command position and the current position. 82 | 83 | ### Complete description 84 | 85 | #### mechanical_calibration_controller 86 | 87 | ```yaml 88 | trigger_calibration_controller: 89 | type: rm_calibration_controllers/MechanicalCalibrationController 90 | actuator: [ trigger_joint_motor ] 91 | velocity: 92 | search_velocity: 4.0 93 | vel_threshold: 0.001 94 | joint: trigger_joint 95 | pid: { p: 0.8, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } 96 | ``` 97 | 98 | #### gpio_calibration_controller 99 | ```yaml 100 | yaw_calibration_controller: 101 | type: rm_calibration_controllers/GpioCalibrationController 102 | actuator: [ yaw_joint_motor ] 103 | gpio: "yaw" 104 | initial_gpio_state: false 105 | velocity: 106 | search_velocity: -4.0 107 | slow_forward_velocity: -2.0 108 | joint: yaw_joint 109 | pid: { p: 0.19, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } 110 | position: 111 | pos_threshold: 0.01 112 | backward_angle: -0.15 113 | joint: yaw_joint 114 | pid: { p: 7.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } 115 | ``` 116 | 117 | ## Bugs & Feature Requests 118 | 119 | Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). 120 | -------------------------------------------------------------------------------- /rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by guanlin on 23-3-14. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace rm_calibration_controllers 18 | { 19 | template 20 | class CalibrationBase : public controller_interface::MultiInterfaceController 21 | { 22 | public: 23 | CalibrationBase() = default; 24 | /** @brief Get necessary params from param server. Init joint_calibration_controller. 25 | * 26 | * Get params from param server and check whether these params are set.Init JointVelocityController.Check 27 | * whether threshold is set correctly. 28 | * 29 | * @param robot_hw The robot hardware abstraction. 30 | * @param root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are 31 | * setup (publishers, subscribers, services). 32 | * @param controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific 33 | * configuration resides. 34 | * @return True if init successful, false when failed. 35 | */ 36 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 37 | /** @brief Switch all of the actuators state to INITIALIZED. 38 | * 39 | * Switch all of the actuator state to INITIALIZED in order to restart the calibration. 40 | * 41 | * @param time The current time. 42 | */ 43 | void starting(const ros::Time& time) override; 44 | void stopping(const ros::Time& time) override; 45 | 46 | protected: 47 | /** @brief Provide a service to know the state of target actuators. 48 | * 49 | * When requesting to this server, it will return respond about whether target actuators has been calibrated. 50 | * 51 | * @param req The request of knowing the state of target actuators. 52 | * @param resp The respond included the state of target actuators. 53 | * @return True if get respond successfully, false when failed. 54 | */ 55 | bool isCalibrated(control_msgs::QueryCalibrationState::Request& req, 56 | control_msgs::QueryCalibrationState::Response& resp); 57 | ros::ServiceServer is_calibrated_srv_; 58 | enum State 59 | { 60 | INITIALIZED, 61 | CALIBRATED 62 | }; 63 | int state_{}; 64 | double velocity_search_{}; 65 | bool calibration_success_ = false; 66 | rm_control::ActuatorExtraHandle actuator_; 67 | effort_controllers::JointVelocityController velocity_ctrl_; 68 | effort_controllers::JointPositionController position_ctrl_; 69 | }; 70 | 71 | } // namespace rm_calibration_controllers 72 | -------------------------------------------------------------------------------- /rm_calibration_controllers/include/rm_calibration_controllers/differential_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by cch on 24-8-7. 36 | // 37 | 38 | #pragma once 39 | #include "effort_controllers/joint_position_controller.h" 40 | #include "effort_controllers/joint_effort_controller.h" 41 | #include "rm_calibration_controllers/calibration_base.h" 42 | 43 | namespace rm_calibration_controllers 44 | { 45 | class DifferentialCalibrationController 46 | : public CalibrationBase 47 | { 48 | public: 49 | DifferentialCalibrationController() = default; 50 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 51 | void update(const ros::Time& time, const ros::Duration& period) override; 52 | 53 | private: 54 | enum State 55 | { 56 | MOVING_POSITIVE = 3, 57 | MOVING_NEGATIVE, 58 | }; 59 | ros::Time start_time_; 60 | int countdown_{}; 61 | double velocity_threshold_{}, max_calibretion_time_{}, position_threshold_{}; 62 | rm_control::ActuatorExtraHandle actuator2_; 63 | effort_controllers::JointVelocityController velocity_ctrl2_; 64 | effort_controllers::JointEffortController effort_ctrl_; 65 | effort_controllers::JointPositionController position_ctrl2_; 66 | }; 67 | } // namespace rm_calibration_controllers 68 | -------------------------------------------------------------------------------- /rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by guanlin on 22-11-7. 3 | // 4 | 5 | #pragma once 6 | 7 | #include "rm_calibration_controllers/calibration_base.h" 8 | #include 9 | 10 | namespace rm_calibration_controllers 11 | { 12 | class GpioCalibrationController 13 | : public CalibrationBase 15 | { 16 | public: 17 | GpioCalibrationController() = default; 18 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 19 | void update(const ros::Time& time, const ros::Duration& period) override; 20 | 21 | private: 22 | enum State 23 | { 24 | FAST_FORWARD = 3, 25 | RETREAT, 26 | SLOW_FORWARD 27 | }; 28 | double position_threshold_{}, backward_angle_{}, start_retreat_position_{}, slow_forward_velocity_{}; 29 | rm_control::GpioStateHandle gpio_state_handle_; 30 | bool initial_gpio_state_; 31 | }; 32 | } // namespace rm_calibration_controllers 33 | -------------------------------------------------------------------------------- /rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 5/16/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include "rm_calibration_controllers/calibration_base.h" 42 | 43 | namespace rm_calibration_controllers 44 | { 45 | class MechanicalCalibrationController 46 | : public CalibrationBase 47 | { 48 | public: 49 | MechanicalCalibrationController() = default; 50 | /** @brief Get necessary params from param server. Init joint_calibration_controller. 51 | * 52 | * Get params from param server and check whether these params are set.Init JointVelocityController.Check 53 | * whether threshold is set correctly. 54 | * 55 | * @param robot_hw The robot hardware abstraction. 56 | * @param root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are 57 | * setup (publishers, subscribers, services). 58 | * @param controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific 59 | * configuration resides. 60 | * @return True if init successful, false when failed. 61 | */ 62 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 63 | /** @brief Execute corresponding action according to current calibration controller state. 64 | * 65 | * Execute corresponding action according to current joint state. If INITIALIZED, target joint will be set 66 | * a vel_search_ and countdown_ to move, and switch state to MOVING. If MOVING, target joint will move until 67 | * current velocity lower than threshold last for a while, and switch state to CALIBRATED. If CALIBRATED, 68 | * target joint velocity will be set to zero and wait for next command. 69 | * 70 | * @param time The current time. 71 | * @param period The time passed since the last call to update. 72 | */ 73 | void update(const ros::Time& time, const ros::Duration& period) override; 74 | 75 | private: 76 | enum State 77 | { 78 | MOVING_POSITIVE = 3, 79 | MOVING_NEGATIVE, 80 | }; 81 | int countdown_{}; 82 | double velocity_threshold_{}, position_threshold_{}; 83 | double positive_position_{}, negative_position_{}, target_position_{}; 84 | bool is_return_{}, is_center_{}; 85 | }; 86 | 87 | } // namespace rm_calibration_controllers 88 | -------------------------------------------------------------------------------- /rm_calibration_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_calibration_controllers 4 | 0.1.11 5 | RoboMaster standard robot Gimbal controller 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | 15 | rm_common 16 | 17 | controller_interface 18 | effort_controllers 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rm_calibration_controllers/rm_calibration_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 9 | 12 | 13 | -------------------------------------------------------------------------------- /rm_calibration_controllers/src/calibration_base.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by guanlin on 23-3-14. 3 | // 4 | 5 | #include "rm_calibration_controllers/calibration_base.h" 6 | 7 | namespace rm_calibration_controllers 8 | { 9 | template class CalibrationBase; 10 | template class CalibrationBase; 12 | 13 | template 14 | bool CalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 15 | ros::NodeHandle& controller_nh) 16 | { 17 | ros::NodeHandle vel_nh(controller_nh, "velocity"); 18 | velocity_ctrl_.init(robot_hw->get(), vel_nh); 19 | XmlRpc::XmlRpcValue actuator; 20 | if (!controller_nh.getParam("actuator", actuator)) 21 | { 22 | ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); 23 | return false; 24 | } 25 | actuator_ = robot_hw->get()->getHandle(actuator[0]); 26 | if (!vel_nh.getParam("search_velocity", velocity_search_)) 27 | { 28 | ROS_ERROR("Search velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 29 | return false; 30 | } 31 | // advertise service to check calibration 32 | is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &CalibrationBase::isCalibrated, this); 33 | return true; 34 | } 35 | 36 | template 37 | void CalibrationBase::starting(const ros::Time& time) 38 | { 39 | actuator_.setCalibrated(false); 40 | state_ = INITIALIZED; 41 | if (actuator_.getCalibrated()) 42 | ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", 43 | velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); 44 | } 45 | 46 | template 47 | void CalibrationBase::stopping(const ros::Time& time) 48 | { 49 | calibration_success_ = false; 50 | } 51 | 52 | template 53 | bool CalibrationBase::isCalibrated(control_msgs::QueryCalibrationState::Request& req, 54 | control_msgs::QueryCalibrationState::Response& resp) 55 | { 56 | ROS_DEBUG("Is calibrated service %d", calibration_success_); 57 | resp.is_calibrated = calibration_success_; 58 | return true; 59 | } 60 | } // namespace rm_calibration_controllers 61 | -------------------------------------------------------------------------------- /rm_calibration_controllers/src/differential_calibration_controller.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by cch on 24-8-7. 36 | // 37 | 38 | #include "rm_calibration_controllers/differential_calibration_controller.h" 39 | 40 | #include 41 | 42 | namespace rm_calibration_controllers 43 | { 44 | bool DifferentialCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 45 | ros::NodeHandle& controller_nh) 46 | { 47 | CalibrationBase::init(robot_hw, root_nh, controller_nh); 48 | XmlRpc::XmlRpcValue actuator; 49 | ros::NodeHandle nh2(controller_nh, "joint2"); 50 | position_ctrl2_.init(robot_hw->get(), nh2); 51 | if (!controller_nh.getParam("max_calibretion_time", max_calibretion_time_)) 52 | { 53 | max_calibretion_time_ = 5.0; 54 | ROS_ERROR("No given max calibration time, set to default (5s)."); 55 | } 56 | if (!controller_nh.getParam("actuator", actuator)) 57 | { 58 | ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); 59 | return false; 60 | } 61 | actuator2_ = robot_hw->get()->getHandle(actuator[1]); 62 | if (!controller_nh.getParam("velocity/vel_threshold", velocity_threshold_)) 63 | { 64 | ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 65 | return false; 66 | } 67 | if (velocity_threshold_ < 0) 68 | { 69 | velocity_threshold_ *= -1.; 70 | ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", 71 | velocity_ctrl_.getJointName().c_str()); 72 | } 73 | calibration_success_ = false; 74 | return true; 75 | } 76 | 77 | void DifferentialCalibrationController::update(const ros::Time& time, const ros::Duration& period) 78 | { 79 | switch (state_) 80 | { 81 | case INITIALIZED: 82 | { 83 | start_time_ = ros::Time::now(); 84 | velocity_ctrl_.setCommand(velocity_search_); 85 | position_ctrl2_.setCommand(position_ctrl2_.getPosition()); 86 | countdown_ = 100; 87 | state_ = MOVING_POSITIVE; 88 | break; 89 | } 90 | case MOVING_POSITIVE: 91 | { 92 | if (std::abs(velocity_ctrl_.joint_.getVelocity()) < velocity_threshold_) 93 | countdown_--; 94 | else 95 | countdown_ = 100; 96 | if (countdown_ < 0 || (ros::Time::now() - start_time_).toSec() > max_calibretion_time_) 97 | { 98 | velocity_ctrl_.setCommand(0); 99 | actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); 100 | actuator2_.setOffset(-actuator2_.getPosition() + actuator2_.getOffset()); 101 | actuator_.setCalibrated(true); 102 | actuator2_.setCalibrated(true); 103 | ROS_INFO_STREAM("Joint " << velocity_ctrl_.getJointName() << " and " << position_ctrl2_.getJointName() 104 | << " are calibrated."); 105 | state_ = CALIBRATED; 106 | velocity_ctrl_.joint_.setCommand(0.); 107 | position_ctrl2_.joint_.setCommand(0.); 108 | position_ctrl2_.setCommand(0.); 109 | calibration_success_ = true; 110 | } 111 | velocity_ctrl_.update(time, period); 112 | position_ctrl2_.update(time, period); 113 | break; 114 | } 115 | case CALIBRATED: 116 | { 117 | velocity_ctrl_.update(time, period); 118 | position_ctrl2_.update(time, period); 119 | break; 120 | } 121 | } 122 | } 123 | } // namespace rm_calibration_controllers 124 | 125 | PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::DifferentialCalibrationController, 126 | controller_interface::ControllerBase) 127 | -------------------------------------------------------------------------------- /rm_calibration_controllers/src/gpio_calibration_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by guanlin on 22-11-7. 3 | // 4 | 5 | #include "rm_calibration_controllers/gpio_calibration_controller.h" 6 | #include 7 | 8 | namespace rm_calibration_controllers 9 | { 10 | bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 11 | ros::NodeHandle& controller_nh) 12 | { 13 | CalibrationBase::init(robot_hw, root_nh, controller_nh); 14 | ros::NodeHandle pos_nh(controller_nh, "position"); 15 | position_ctrl_.init(robot_hw->get(), pos_nh); 16 | if (!pos_nh.getParam("pos_threshold", position_threshold_)) 17 | { 18 | ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 19 | return false; 20 | } 21 | if (!pos_nh.getParam("backward_angle", backward_angle_)) 22 | { 23 | ROS_ERROR("Backward angle was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 24 | return false; 25 | } 26 | if (!controller_nh.getParam("velocity/slow_forward_velocity", slow_forward_velocity_)) 27 | { 28 | ROS_ERROR("Slow forward velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 29 | return false; 30 | } 31 | std::string gpio{}; 32 | if (!controller_nh.getParam("gpio", gpio)) 33 | { 34 | ROS_ERROR("No gpio given (namespace: %s)", controller_nh.getNamespace().c_str()); 35 | return false; 36 | } 37 | if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) 38 | { 39 | ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); 40 | return false; 41 | } 42 | gpio_state_handle_ = robot_hw->get()->getHandle(gpio); 43 | return true; 44 | } 45 | 46 | void GpioCalibrationController::update(const ros::Time& time, const ros::Duration& period) 47 | { 48 | switch (state_) 49 | { 50 | case INITIALIZED: 51 | { 52 | velocity_ctrl_.setCommand(velocity_search_); 53 | state_ = FAST_FORWARD; 54 | break; 55 | } 56 | case FAST_FORWARD: 57 | { 58 | if (gpio_state_handle_.getValue() != initial_gpio_state_) 59 | { 60 | start_retreat_position_ = velocity_ctrl_.joint_.getPosition(); 61 | velocity_ctrl_.setCommand(0); 62 | state_ = RETREAT; 63 | } 64 | else 65 | velocity_ctrl_.update(time, period); 66 | break; 67 | } 68 | case RETREAT: 69 | { 70 | position_ctrl_.setCommand(start_retreat_position_ - backward_angle_); 71 | position_ctrl_.update(time, period); 72 | if (std::abs(position_ctrl_.command_struct_.position_ - position_ctrl_.joint_.getPosition()) < position_threshold_) 73 | state_ = SLOW_FORWARD; 74 | break; 75 | } 76 | case SLOW_FORWARD: 77 | { 78 | velocity_ctrl_.setCommand(slow_forward_velocity_); 79 | if (gpio_state_handle_.getValue() != initial_gpio_state_) 80 | { 81 | velocity_ctrl_.setCommand(0); 82 | actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); 83 | actuator_.setCalibrated(true); 84 | ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); 85 | state_ = CALIBRATED; 86 | } 87 | velocity_ctrl_.update(time, period); 88 | break; 89 | } 90 | case CALIBRATED: 91 | calibration_success_ = true; 92 | } 93 | } 94 | } // namespace rm_calibration_controllers 95 | 96 | PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::GpioCalibrationController, controller_interface::ControllerBase) 97 | -------------------------------------------------------------------------------- /rm_calibration_controllers/src/mechanical_calibration_controller.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 5/16/21. 36 | // 37 | 38 | #include "rm_calibration_controllers/mechanical_calibration_controller.h" 39 | 40 | #include 41 | 42 | namespace rm_calibration_controllers 43 | { 44 | bool MechanicalCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 45 | ros::NodeHandle& controller_nh) 46 | { 47 | CalibrationBase::init(robot_hw, root_nh, controller_nh); 48 | is_return_ = is_center_ = false; 49 | controller_nh.getParam("center", is_center_); 50 | if (!controller_nh.getParam("velocity/vel_threshold", velocity_threshold_)) 51 | { 52 | ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); 53 | return false; 54 | } 55 | if (velocity_threshold_ < 0) 56 | { 57 | velocity_threshold_ *= -1.; 58 | ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", 59 | velocity_ctrl_.getJointName().c_str()); 60 | } 61 | if (controller_nh.hasParam("return")) 62 | { 63 | ros::NodeHandle nh_return(controller_nh, "return"); 64 | position_ctrl_.init(robot_hw->get(), nh_return); 65 | if (!nh_return.getParam("target_position", target_position_)) 66 | { 67 | ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); 68 | return false; 69 | } 70 | if (!nh_return.getParam("pos_threshold", position_threshold_)) 71 | { 72 | ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); 73 | return false; 74 | } 75 | is_return_ = true; 76 | calibration_success_ = false; 77 | } 78 | return true; 79 | } 80 | 81 | void MechanicalCalibrationController::update(const ros::Time& time, const ros::Duration& period) 82 | { 83 | switch (state_) 84 | { 85 | case INITIALIZED: 86 | { 87 | velocity_ctrl_.setCommand(velocity_search_); 88 | countdown_ = 100; 89 | state_ = MOVING_POSITIVE; 90 | break; 91 | } 92 | case MOVING_POSITIVE: 93 | { 94 | if (std::abs(velocity_ctrl_.joint_.getVelocity()) < velocity_threshold_ && !actuator_.getHalted()) 95 | countdown_--; 96 | else 97 | countdown_ = 100; 98 | if (countdown_ < 0) 99 | { 100 | velocity_ctrl_.setCommand(0); 101 | if (!is_center_) 102 | { 103 | actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); 104 | actuator_.setCalibrated(true); 105 | ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); 106 | state_ = CALIBRATED; 107 | if (is_return_) 108 | { 109 | position_ctrl_.setCommand(target_position_); 110 | } 111 | else 112 | { 113 | velocity_ctrl_.joint_.setCommand(0.); 114 | calibration_success_ = true; 115 | } 116 | } 117 | else 118 | { 119 | positive_position_ = actuator_.getPosition(); 120 | countdown_ = 100; 121 | velocity_ctrl_.setCommand(-velocity_search_); 122 | state_ = MOVING_NEGATIVE; 123 | } 124 | } 125 | velocity_ctrl_.update(time, period); 126 | break; 127 | } 128 | case MOVING_NEGATIVE: 129 | { 130 | if (std::abs(velocity_ctrl_.joint_.getVelocity()) < velocity_threshold_) 131 | countdown_--; 132 | else 133 | countdown_ = 100; 134 | if (countdown_ < 0) 135 | { 136 | velocity_ctrl_.setCommand(0); 137 | negative_position_ = actuator_.getPosition(); 138 | actuator_.setOffset(-(positive_position_ + negative_position_) / 2 + actuator_.getOffset()); 139 | actuator_.setCalibrated(true); 140 | ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); 141 | state_ = CALIBRATED; 142 | if (is_return_) 143 | position_ctrl_.setCommand(target_position_); 144 | else 145 | { 146 | velocity_ctrl_.joint_.setCommand(0.); 147 | calibration_success_ = true; 148 | } 149 | } 150 | velocity_ctrl_.update(time, period); 151 | break; 152 | } 153 | case CALIBRATED: 154 | { 155 | if (is_return_) 156 | { 157 | if ((std::abs(position_ctrl_.joint_.getPosition() - target_position_)) < position_threshold_) 158 | calibration_success_ = true; 159 | position_ctrl_.update(time, period); 160 | } 161 | else 162 | velocity_ctrl_.update(time, period); 163 | break; 164 | } 165 | } 166 | } 167 | } // namespace rm_calibration_controllers 168 | 169 | PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::MechanicalCalibrationController, controller_interface::ControllerBase) 170 | -------------------------------------------------------------------------------- /rm_chassis_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rm_chassis_controllers) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror -Wno-enum-compare) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | roscpp 16 | 17 | rm_common 18 | 19 | controller_interface 20 | effort_controllers 21 | tf2_geometry_msgs 22 | nav_msgs 23 | angles 24 | ) 25 | 26 | find_package(Eigen3 REQUIRED) 27 | 28 | ################################### 29 | ## catkin specific configuration ## 30 | ################################### 31 | ## The catkin_package macro generates cmake config files for your package 32 | ## Declare things to be passed to dependent projects 33 | ## INCLUDE_DIRS: uncomment this if your package contains header files 34 | ## LIBRARIES: libraries you create in this project that dependent projects also need 35 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 36 | ## DEPENDS: system dependencies of this project that dependent projects also need 37 | catkin_package( 38 | INCLUDE_DIRS 39 | include 40 | ${EIGEN3_INCLUDE_DIR} 41 | LIBRARIES 42 | CATKIN_DEPENDS 43 | roscpp 44 | 45 | rm_common 46 | 47 | effort_controllers 48 | tf2_geometry_msgs 49 | nav_msgs 50 | angles 51 | LIBRARIES ${PROJECT_NAME} 52 | ) 53 | 54 | ########### 55 | ## Build ## 56 | ########### 57 | 58 | ## Specify additional locations of header files 59 | include_directories( 60 | include 61 | ${catkin_INCLUDE_DIRS} 62 | ${EIGEN3_INCLUDE_DIR} 63 | ) 64 | 65 | ## Declare cpp executables 66 | add_library(${PROJECT_NAME} 67 | src/chassis_base.cpp 68 | src/omni.cpp 69 | src/sentry.cpp 70 | src/swerve.cpp 71 | src/balance.cpp 72 | ) 73 | 74 | ## Specify libraries to link executable targets against 75 | target_link_libraries(${PROJECT_NAME} 76 | ${catkin_LIBRARIES} 77 | ) 78 | 79 | ############# 80 | ## Install ## 81 | ############# 82 | 83 | # Mark executables and/or libraries for installation 84 | install( 85 | TARGETS ${PROJECT_NAME} 86 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | 91 | # Mark cpp header files for installation 92 | install( 93 | DIRECTORY include/${PROJECT_NAME}/ 94 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 95 | FILES_MATCHING PATTERN "*.h" 96 | ) 97 | 98 | # Mark other files for installation 99 | install( 100 | DIRECTORY test 101 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 102 | ) 103 | install( 104 | FILES rm_chassis_controllers_plugins.xml 105 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 106 | ) 107 | 108 | ############# 109 | ## Testing ## 110 | ############# 111 | 112 | #if (${CATKIN_ENABLE_TESTING}) 113 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 114 | # ## Add gtest based cpp test target and link libraries 115 | # catkin_add_gtest(${PROJECT_NAME}-test 116 | # test/test_ros_package_template.cpp 117 | # test/AlgorithmTest.cpp) 118 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 119 | #endif () 120 | -------------------------------------------------------------------------------- /rm_chassis_controllers/README.md: -------------------------------------------------------------------------------- 1 | # rm_chassis_controllers 2 | 3 | ## Overview 4 | 5 | There are three states: raw, follow and twist. The output torque and speed of each motor of the chassis can be 6 | calculated according to the current state of the control, the received speed and pose of the pan/tilt, and the speed and 7 | acceleration commands, and the data is returned by the motor to calculate The speed and posture of the chassis are 8 | released. The control algorithm involved in the chassis controller is PID algorithm. 9 | 10 | **Keywords:** mecanum, swerve, balance, chassis, ROS, RoboMaster 11 | 12 | ### Hardware interface type 13 | 14 | + `JointStateInterface` Used to get the position and speed of chassis wheel joint. 15 | 16 | + `EffortJointInterface` Used to send the torque command of chassis wheel joint. 17 | 18 | + `RoboSateInterface` Used for high-frequency maintenance of the transformation relationship of changing odom to 19 | base_link. 20 | 21 | ## Installation 22 | 23 | ### Installation from Packages 24 | 25 | To install all packages from the this repository as Debian packages use 26 | 27 | ``` 28 | sudo apt-get install ros-noetic-rm-chassis-controllers 29 | ``` 30 | 31 | Or better, use `rosdep`: 32 | 33 | ``` 34 | sudo rosdep install --from-paths src 35 | ``` 36 | 37 | ### Dependencies 38 | 39 | * [Robot Operating System (ROS)](http://wiki.ros.org/) (middleware for robotics), 40 | * roscpp 41 | * rm_common 42 | * controller_interface 43 | * effort_controllers 44 | * tf2_geometry_msgs 45 | * angles 46 | * robot_localization 47 | 48 | ## ROS API 49 | 50 | #### Subscribed Topics 51 | 52 | * **`base_imu`** ([sensor_msgs/Imu](http://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) 53 | 54 | The inertial measurement unit data of base command. 55 | 56 | * **`command`** (rm_msgs::ChassisCmd) 57 | 58 | Set the mode, acceleration, and maximum power of the chassis. 59 | 60 | * **`cmd_vel`** ([geometry_msgs/Twist](http://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html)) 61 | 62 | Set the speed of the chassis. 63 | 64 | #### Published Topics 65 | 66 | * **`odom`**([nav_msgs/Odometry](http://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html)) 67 | 68 | Chassis odometer information (speed, position, covariance). 69 | 70 | * **`state`**([rm_msgs::BalanceState](http://docs.ros.org/en/api/rm_msgs/html/msg/BalanceState.html)) 71 | 72 | Contains quantities of state and control about the Balance. 73 | 74 | #### Parameters 75 | 76 | ##### common 77 | 78 | * **`wheel_radius`** (double) 79 | 80 | Radius of the wheels. 81 | 82 | * **`wheel_track`** (double) 83 | 84 | The distance between the center of the left and right wheels on the same side. 85 | 86 | * **`wheel_base`** (double) 87 | 88 | The distance between the center of the front and rear wheels on the same side. 89 | 90 | * **`twist_angle`** (double) 91 | 92 | Amplitude of twist at `twist` state. 93 | 94 | * **`enable_odom_tf`** (bool, default: true) 95 | 96 | Option.If it is set to true, it will store Transform in tf_buffer. 97 | 98 | * **`publish_odom_tf_`** (bool, default: false) 99 | 100 | Option.If it is set to true, enable_odom_tf is also true, it will send Transform from odom to base. 101 | 102 | * **`twist_covariance_diagonal`** (double[6]) 103 | 104 | The diagonal covariance matrix of twist. 105 | 106 | * **`publish_rate`** (double, default: 50) 107 | 108 | Frequency (in Hz) of publishing Transform. 109 | 110 | * **`coeff`** (double) 111 | 112 | A coefficient. Adjust this coefficient to reduce the impact of power loss. 113 | 114 | * **`min_vel`** (double) 115 | 116 | The minimum velocity of chassis joint which is used to calculate the max torque. 117 | 118 | * **`timeout`** (double) 119 | 120 | Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0. 121 | 122 | * **`power_offset`** (double) 123 | 124 | Fix the difference between theoretical power and actual power. 125 | 126 | ##### Balance 127 | 128 | * **`imu_name`** (string, default: "base_imu") 129 | 130 | Chassis imu name. 131 | 132 | * **`left/wheel_joint`** (string, default: "left_wheel_joint") 133 | 134 | left wheel joint name. 135 | 136 | * **`left/block_joint`** (string, default: "left_momentum_block_joint") 137 | 138 | left momentum block joint name. 139 | 140 | * **`right/wheel_joint`** (string, default: "right_wheel_joint") 141 | 142 | right wheel joint name. 143 | 144 | * **`right/block_joint`** (string, default: "right_momentum_block_joint") 145 | 146 | right momentum block joint name. 147 | 148 | * **`m_w`** (double, default: 0.72) 149 | 150 | mass of single wheel. 151 | 152 | * **`m`** (double, default: 11.48) 153 | 154 | mass of the robot except wheels and momentum_blocks. 155 | 156 | * **`m_b`** (double, default: 1.13) 157 | 158 | mass of single momentum_block. 159 | 160 | * **`i_w`** (double, default: 0.01683) 161 | 162 | The moment of inertia of the wheel around the rotational axis of the motor. 163 | 164 | * **`l`** (double, default: 0.0587) 165 | 166 | The vertical component of the distance between the wheel center and the center of mass of robot. 167 | 168 | * **`y_b`** (double, default: 0.16) 169 | 170 | The y-axis component of the coordinates of the momentum block in the base_link coordinate system. 171 | 172 | * **`z_b`** (double[4], default: 0.0468) 173 | 174 | The vertical component of the distance between the momentum block and the center of mass of robot. 175 | 176 | * **`g`** (double, default: 9.8) 177 | 178 | Gravity constant. 179 | 180 | * **`i_m`** (double, default: 0.1982) 181 | 182 | The moment of inertia of the robot around the y-axis of base_link coordinate. 183 | 184 | * **`q`** (double[16]) 185 | 186 | Weight matrix. 187 | 188 | * **`r`** (double[4]) 189 | 190 | Weight matrix. 191 | 192 | ##### Swerve 193 | 194 | * **`/modules//position`** (double[2]) 195 | 196 | The position of module. 197 | 198 | * **`/modules//pivot/joint`** (string) 199 | 200 | Joint between chassis and privot. 201 | 202 | * **`/modules//pivot/offset`** (double) 203 | 204 | Angle between the wheel rotation axis and the chassis's Y axis. 205 | 206 | * **`/modules//wheel/joint`** (string) 207 | 208 | Joint between privot and wheel. 209 | 210 | * **`/modules//wheel/radius`** (double) 211 | 212 | The radius of wheel. 213 | 214 | ##### Omni 215 | 216 | * **`/wheels//pose`** (double[3]) 217 | 218 | The pose of wheel. 219 | 220 | * **`/wheels//joint`** (string) 221 | 222 | wheel joint name. 223 | 224 | * **`/wheels/left_front/roller_angle`** (double) 225 | 226 | The roller angle of wheel. 227 | 228 | * **`/wheels/left_front/radius`** (double) 229 | 230 | The radius of wheel. 231 | 232 | ## Controller configuration examples 233 | 234 | ### Complete description 235 | 236 | ``` 237 | chassis_controller: 238 | type: rm_chassis_controllers/OmniController 239 | publish_rate: 100 240 | enable_odom_tf: true 241 | publish_odom_tf: false 242 | power: 243 | effort_coeff: 10.0 244 | vel_coeff: 0.003 245 | power_offset: -8.41 246 | twist_angular: 0.5233 247 | timeout: 0.1 248 | pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 249 | twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] 250 | 251 | wheels: 252 | left_front: 253 | pose: [ 0.147, 0.147, 2.356 ] 254 | joint: left_front_wheel_joint 255 | <<: &wheel_setting 256 | roller_angle: 0. 257 | radius: 0.07625 258 | pid: { p: 0.41, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 259 | right_front: 260 | pose: [ 0.147, -0.147, 0.785 ] 261 | joint: right_front_wheel_joint 262 | <<: *wheel_setting 263 | left_back: 264 | pose: [ -0.147, 0.147, -2.356 ] 265 | joint: left_back_wheel_joint 266 | <<: *wheel_setting 267 | right_back: 268 | pose: [ -0.147, -0.147, -0.785 ] 269 | joint: right_back_wheel_joint 270 | <<: *wheel_setting 271 | ``` 272 | 273 | ## Bugs & Feature Requests 274 | 275 | Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). 276 | -------------------------------------------------------------------------------- /rm_chassis_controllers/include/rm_chassis_controllers/balance.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2022/11/15. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "rm_chassis_controllers/chassis_base.h" 14 | 15 | namespace rm_chassis_controllers 16 | { 17 | using Eigen::Matrix; 18 | class BalanceController : public ChassisBase 20 | { 21 | enum BalanceMode 22 | { 23 | NORMAL, 24 | BLOCK 25 | }; 26 | 27 | public: 28 | BalanceController() = default; 29 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 30 | 31 | private: 32 | void moveJoint(const ros::Time& time, const ros::Duration& period) override; 33 | void normal(const ros::Time& time, const ros::Duration& period); 34 | void block(const ros::Time& time, const ros::Duration& period); 35 | geometry_msgs::Twist odometry() override; 36 | static const int STATE_DIM = 10; 37 | static const int CONTROL_DIM = 4; 38 | Eigen::Matrix k_{}; 39 | Eigen::Matrix a_{}, q_{}; 40 | Eigen::Matrix b_{}; 41 | Eigen::Matrix r_{}; 42 | Eigen::Matrix x_; 43 | double wheel_radius_, wheel_base_; 44 | double position_des_ = 0; 45 | double position_offset_ = 0.; 46 | double position_clear_threshold_ = 0.; 47 | double yaw_des_ = 0; 48 | 49 | int balance_mode_; 50 | ros::Time block_time_, last_block_time_; 51 | double block_angle_, block_duration_, block_velocity_, block_effort_, anti_block_effort_, block_overtime_; 52 | bool balance_state_changed_ = false, maybe_block_ = false; 53 | 54 | hardware_interface::ImuSensorHandle imu_handle_; 55 | hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_, 56 | left_momentum_block_joint_handle_, right_momentum_block_joint_handle_; 57 | 58 | typedef std::shared_ptr> RtpublisherPtr; 59 | RtpublisherPtr state_pub_; 60 | geometry_msgs::Vector3 angular_vel_base_; 61 | double roll_, pitch_, yaw_; 62 | }; 63 | 64 | } // namespace rm_chassis_controllers 65 | -------------------------------------------------------------------------------- /rm_chassis_controllers/include/rm_chassis_controllers/omni.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/29. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include "rm_chassis_controllers/chassis_base.h" 10 | 11 | namespace rm_chassis_controllers 12 | { 13 | class OmniController : public ChassisBase 14 | { 15 | public: 16 | OmniController() = default; 17 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 18 | 19 | private: 20 | void moveJoint(const ros::Time& time, const ros::Duration& period) override; 21 | geometry_msgs::Twist odometry() override; 22 | 23 | std::vector> joints_; 24 | Eigen::MatrixXd chassis2joints_; 25 | }; 26 | 27 | } // namespace rm_chassis_controllers 28 | -------------------------------------------------------------------------------- /rm_chassis_controllers/include/rm_chassis_controllers/sentry.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by flying on 2021/1/18. 36 | // 37 | 38 | #pragma once 39 | 40 | #include "rm_chassis_controllers/chassis_base.h" 41 | #include 42 | 43 | namespace rm_chassis_controllers 44 | { 45 | class SentryController : public ChassisBase 46 | { 47 | public: 48 | SentryController() = default; 49 | /** @brief Execute ChassisBase::init. Init necessary handles. 50 | * 51 | * @param robot_hw The robot hardware abstraction. 52 | * @param root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are 53 | * setup (publishers, subscribers, services). 54 | * @param controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific 55 | * configuration resides. 56 | * @return True if initialization was successful and the controller 57 | * is ready to be started. 58 | */ 59 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 60 | 61 | private: 62 | /** @brief Calculate correct command and set it to wheel. 63 | * 64 | * @param time The current time. 65 | * @param period The time passed since the last call to update. 66 | */ 67 | void moveJoint(const ros::Time& time, const ros::Duration& period) override; 68 | void catapult(const ros::Time& time, const ros::Duration& period); 69 | void normal(const ros::Time& time, const ros::Duration& period); 70 | /** @brief Calculate current linear_x according to current velocity. 71 | * 72 | * @return Calculated vel_data included linear_x. 73 | */ 74 | geometry_msgs::Twist odometry() override; 75 | 76 | effort_controllers::JointVelocityController ctrl_wheel_; 77 | effort_controllers::JointPositionController ctrl_catapult_joint_; 78 | 79 | bool if_catapult_; 80 | double catapult_initial_velocity_; 81 | double catapult_angle_; 82 | double vel_coff_; 83 | double last_vel_cmd_{ 0. }; 84 | ros::Time lock_time_; 85 | double lock_duratoin_; 86 | }; 87 | 88 | } // namespace rm_chassis_controllers 89 | -------------------------------------------------------------------------------- /rm_chassis_controllers/include/rm_chassis_controllers/swerve.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 4/23/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include "rm_chassis_controllers/chassis_base.h" 41 | 42 | #include 43 | #include 44 | 45 | namespace rm_chassis_controllers 46 | { 47 | struct Module 48 | { 49 | Vec2 position_; 50 | double pivot_offset_, wheel_radius_; 51 | effort_controllers::JointPositionController* ctrl_pivot_; 52 | effort_controllers::JointVelocityController* ctrl_wheel_; 53 | }; 54 | 55 | class SwerveController : public ChassisBase 56 | { 57 | public: 58 | SwerveController() = default; 59 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 60 | 61 | private: 62 | void moveJoint(const ros::Time& time, const ros::Duration& period) override; 63 | geometry_msgs::Twist odometry() override; 64 | std::vector modules_; 65 | }; 66 | 67 | } // namespace rm_chassis_controllers 68 | -------------------------------------------------------------------------------- /rm_chassis_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_chassis_controllers 4 | 0.1.11 5 | RoboMaster standard robot Chassis controller 6 | Qiayuan Liao 7 | BSD 8 | 9 | 10 | Qiayuan Liao 11 | 12 | 13 | catkin 14 | 15 | roscpp 16 | 17 | rm_common 18 | 19 | controller_interface 20 | effort_controllers 21 | tf2_geometry_msgs 22 | nav_msgs 23 | angles 24 | 25 | robot_localization 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rm_chassis_controllers/rm_chassis_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The OmniController is Robomaster Omnidirectional wheel Chassis controller. It expects a EffortJointInterface 8 | type of hardware interface. 9 | 10 | 11 | 14 | 15 | The SentryController is RoboMaster sentry robot Chassis controller. It expects a EffortJointInterface 16 | type of hardware interface. 17 | 18 | 19 | 22 | 23 | The BalanceController is RoboMaster swerve drive Chassis controller. It expects a EffortJointInterface 24 | type of hardware interface. 25 | 26 | 27 | 30 | 31 | The BalanceController is RoboMaster balance standard robot Balance controller. It expects a 32 | EffortJointInterface type of hardware interface. 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /rm_chassis_controllers/src/omni.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/29. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "rm_chassis_controllers/omni.h" 12 | 13 | namespace rm_chassis_controllers 14 | { 15 | bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 16 | ros::NodeHandle& controller_nh) 17 | { 18 | ChassisBase::init(robot_hw, root_nh, controller_nh); 19 | 20 | XmlRpc::XmlRpcValue wheels; 21 | controller_nh.getParam("wheels", wheels); 22 | chassis2joints_.resize(wheels.size(), 3); 23 | 24 | size_t i = 0; 25 | for (const auto& wheel : wheels) 26 | { 27 | ROS_ASSERT(wheel.second.hasMember("pose")); 28 | ROS_ASSERT(wheel.second["pose"].getType() == XmlRpc::XmlRpcValue::TypeArray); 29 | ROS_ASSERT(wheel.second["pose"].size() == 3); 30 | ROS_ASSERT(wheel.second.hasMember("roller_angle")); 31 | ROS_ASSERT(wheel.second.hasMember("radius")); 32 | 33 | // Ref: Modern Robotics, Chapter 13.2: Omnidirectional Wheeled Mobile Robots 34 | Eigen::MatrixXd direction(1, 2), in_wheel(2, 2), in_chassis(2, 3); 35 | double beta = (double)wheel.second["pose"][2]; 36 | double roller_angle = (double)wheel.second["roller_angle"]; 37 | direction << 1, tan(roller_angle); 38 | in_wheel << cos(beta), sin(beta), -sin(beta), cos(beta); 39 | in_chassis << -(double)wheel.second["pose"][1], 1., 0., (double)wheel.second["pose"][0], 0., 1.; 40 | Eigen::MatrixXd chassis2joint = 1. / (double)wheel.second["radius"] * direction * in_wheel * in_chassis; 41 | chassis2joints_.block<1, 3>(i, 0) = chassis2joint; 42 | 43 | ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "wheels/" + wheel.first); 44 | joints_.push_back(std::make_shared()); 45 | if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) 46 | return false; 47 | joint_handles_.push_back(joints_[i]->joint_); 48 | 49 | i++; 50 | } 51 | return true; 52 | } 53 | 54 | void OmniController::moveJoint(const ros::Time& time, const ros::Duration& period) 55 | { 56 | Eigen::Vector3d vel_chassis; 57 | vel_chassis << vel_cmd_.z, vel_cmd_.x, vel_cmd_.y; 58 | Eigen::VectorXd vel_joints = chassis2joints_ * vel_chassis; 59 | for (size_t i = 0; i < joints_.size(); i++) 60 | { 61 | joints_[i]->setCommand(vel_joints(i)); 62 | joints_[i]->update(time, period); 63 | } 64 | } 65 | 66 | geometry_msgs::Twist OmniController::odometry() 67 | { 68 | Eigen::VectorXd vel_joints(joints_.size()); 69 | for (size_t i = 0; i < joints_.size(); i++) 70 | vel_joints[i] = joints_[i]->joint_.getVelocity(); 71 | Eigen::Vector3d vel_chassis = 72 | (chassis2joints_.transpose() * chassis2joints_).inverse() * chassis2joints_.transpose() * vel_joints; 73 | geometry_msgs::Twist twist; 74 | twist.angular.z = vel_chassis(0); 75 | twist.linear.x = vel_chassis(1); 76 | twist.linear.y = vel_chassis(2); 77 | return twist; 78 | } 79 | 80 | } // namespace rm_chassis_controllers 81 | PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase) 82 | -------------------------------------------------------------------------------- /rm_chassis_controllers/src/sentry.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by flying on 2021/1/18. 36 | // 37 | #include "rm_chassis_controllers/sentry.h" 38 | #include 39 | #include 40 | #include 41 | 42 | namespace rm_chassis_controllers 43 | { 44 | bool SentryController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 45 | ros::NodeHandle& controller_nh) 46 | { 47 | ChassisBase::init(robot_hw, root_nh, controller_nh); 48 | ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "wheel"); 49 | ros::NodeHandle nh_brake = ros::NodeHandle(controller_nh, "catapult"); 50 | if (!nh_brake.getParam("catapult_angle", catapult_angle_) || !nh_brake.getParam("velocity_coefficient", vel_coff_) || 51 | !nh_brake.getParam("lock_duration", lock_duratoin_)) 52 | { 53 | ROS_ERROR("Could not find parameters: catapult_angle, velocity_coefficient or lock_duration"); 54 | } 55 | if (!ctrl_wheel_.init(effort_joint_interface_, nh_wheel) || 56 | !ctrl_catapult_joint_.init(effort_joint_interface_, nh_brake)) 57 | return false; 58 | if_catapult_ = false; 59 | joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); 60 | joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); 61 | return true; 62 | } 63 | 64 | void SentryController::moveJoint(const ros::Time& time, const ros::Duration& period) 65 | { 66 | ctrl_wheel_.setCommand(vel_cmd_.x / wheel_radius_); 67 | ctrl_catapult_joint_.setCommand(0.); 68 | ctrl_wheel_.update(time, period); 69 | ctrl_catapult_joint_.update(time, period); 70 | 71 | if (last_vel_cmd_ * cmd_rt_buffer_.readFromRT()->cmd_vel_.linear.x < 0) 72 | { 73 | if_catapult_ = true; 74 | ROS_INFO("[sentryChassis] Enter CATAPULT"); 75 | } 76 | last_vel_cmd_ = cmd_rt_buffer_.readFromRT()->cmd_vel_.linear.x; 77 | if (if_catapult_ == false) 78 | { 79 | normal(time, period); 80 | lock_time_ = time; 81 | } 82 | else 83 | { 84 | catapult(time, period); 85 | } 86 | } 87 | 88 | void SentryController::catapult(const ros::Time& time, const ros::Duration& period) 89 | { 90 | ctrl_catapult_joint_.setCommand(catapult_initial_velocity_ > 0 ? -catapult_angle_ : catapult_angle_); 91 | ctrl_wheel_.joint_.setCommand(0.); 92 | ctrl_wheel_.update(time, period); 93 | ctrl_catapult_joint_.update(time, period); 94 | if ((catapult_initial_velocity_ * ctrl_wheel_.joint_.getVelocity() < 0) && 95 | (std::abs(ctrl_wheel_.joint_.getVelocity()) > std::abs(catapult_initial_velocity_ * vel_coff_))) 96 | { 97 | if_catapult_ = false; 98 | ROS_INFO("[sentryChassis] Enter NORMAL"); 99 | } 100 | if ((time - lock_time_).toSec() > lock_duratoin_) 101 | { 102 | ctrl_catapult_joint_.setCommand(0.); 103 | if_catapult_ = false; 104 | ROS_INFO("[sentryChassis] Exit CATAPULT"); 105 | } 106 | } 107 | 108 | geometry_msgs::Twist SentryController::odometry() 109 | { 110 | geometry_msgs::Twist vel_data; 111 | vel_data.linear.x = ctrl_wheel_.joint_.getVelocity() * wheel_radius_; 112 | return vel_data; 113 | } 114 | 115 | void SentryController::normal(const ros::Time& time, const ros::Duration& period) 116 | { 117 | catapult_initial_velocity_ = ctrl_wheel_.joint_.getVelocity(); 118 | } 119 | 120 | } // namespace rm_chassis_controllers 121 | 122 | PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SentryController, controller_interface::ControllerBase) 123 | -------------------------------------------------------------------------------- /rm_chassis_controllers/src/swerve.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 4/23/21. 36 | // 37 | 38 | #include "rm_chassis_controllers/swerve.h" 39 | 40 | #include 41 | #include 42 | 43 | namespace rm_chassis_controllers 44 | { 45 | bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, 46 | ros::NodeHandle& controller_nh) 47 | { 48 | if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) 49 | return false; 50 | XmlRpc::XmlRpcValue modules; 51 | controller_nh.getParam("modules", modules); 52 | ROS_ASSERT(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct); 53 | for (const auto& module : modules) 54 | { 55 | ROS_ASSERT(module.second.hasMember("position")); 56 | ROS_ASSERT(module.second["position"].getType() == XmlRpc::XmlRpcValue::TypeArray); 57 | ROS_ASSERT(module.second["position"].size() == 2); 58 | ROS_ASSERT(module.second.hasMember("pivot")); 59 | ROS_ASSERT(module.second["pivot"].getType() == XmlRpc::XmlRpcValue::TypeStruct); 60 | ROS_ASSERT(module.second.hasMember("wheel")); 61 | ROS_ASSERT(module.second["wheel"].getType() == XmlRpc::XmlRpcValue::TypeStruct); 62 | ROS_ASSERT(module.second["wheel"].hasMember("radius")); 63 | 64 | Module m{ .position_ = Vec2((double)module.second["position"][0], (double)module.second["position"][1]), 65 | .pivot_offset_ = module.second["pivot"]["offset"], 66 | .wheel_radius_ = module.second["wheel"]["radius"], 67 | .ctrl_pivot_ = new effort_controllers::JointPositionController(), 68 | .ctrl_wheel_ = new effort_controllers::JointVelocityController() }; 69 | ros::NodeHandle nh_pivot = ros::NodeHandle(controller_nh, "modules/" + module.first + "/pivot"); 70 | ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "modules/" + module.first + "/wheel"); 71 | if (!m.ctrl_pivot_->init(effort_joint_interface_, nh_pivot) || 72 | !m.ctrl_wheel_->init(effort_joint_interface_, nh_wheel)) 73 | return false; 74 | if (module.second["pivot"].hasMember("offset")) 75 | m.pivot_offset_ = module.second["pivot"]["offset"]; 76 | joint_handles_.push_back(m.ctrl_pivot_->joint_); 77 | joint_handles_.push_back(m.ctrl_wheel_->joint_); 78 | modules_.push_back(m); 79 | } 80 | return true; 81 | } 82 | 83 | // Ref: https://dominik.win/blog/programming-swerve-drive/ 84 | 85 | void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) 86 | { 87 | Vec2 vel_center(vel_cmd_.x, vel_cmd_.y); 88 | for (auto& module : modules_) 89 | { 90 | Vec2 vel = vel_center + vel_cmd_.z * Vec2(-module.position_.y(), module.position_.x()); 91 | double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; 92 | // Direction flipping and Stray module mitigation 93 | double a = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle); 94 | double b = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle + M_PI); 95 | module.ctrl_pivot_->setCommand(std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI); 96 | module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); 97 | module.ctrl_pivot_->update(time, period); 98 | module.ctrl_wheel_->update(time, period); 99 | } 100 | } 101 | 102 | geometry_msgs::Twist SwerveController::odometry() 103 | { 104 | geometry_msgs::Twist vel_data{}; 105 | geometry_msgs::Twist vel_modules{}; 106 | for (auto& module : modules_) 107 | { 108 | geometry_msgs::Twist vel; 109 | vel.linear.x = module.ctrl_wheel_->joint_.getVelocity() * module.wheel_radius_ * 110 | std::cos(module.ctrl_pivot_->joint_.getPosition()); 111 | vel.linear.y = module.ctrl_wheel_->joint_.getVelocity() * module.wheel_radius_ * 112 | std::sin(module.ctrl_pivot_->joint_.getPosition()); 113 | vel.angular.z = 114 | module.ctrl_wheel_->joint_.getVelocity() * module.wheel_radius_ * 115 | std::cos(module.ctrl_pivot_->joint_.getPosition() - std::atan2(module.position_.x(), -module.position_.y())); 116 | vel_modules.linear.x += vel.linear.x; 117 | vel_modules.linear.y += vel.linear.y; 118 | vel_modules.angular.z += vel.angular.z; 119 | } 120 | vel_data.linear.x = vel_modules.linear.x / modules_.size(); 121 | vel_data.linear.y = vel_modules.linear.y / modules_.size(); 122 | vel_data.angular.z = 123 | vel_modules.angular.z / modules_.size() / 124 | std::sqrt(std::pow(modules_.begin()->position_.x(), 2) + std::pow(modules_.begin()->position_.y(), 2)); 125 | return vel_data; 126 | } 127 | 128 | PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) 129 | } // namespace rm_chassis_controllers 130 | -------------------------------------------------------------------------------- /rm_chassis_controllers/test/balance.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | chassis_controller: 3 | type: rm_chassis_controllers/BalanceController 4 | # ChassisBase 5 | publish_rate: 100 6 | enable_odom_tf: true 7 | publish_odom_tf: false 8 | power: 9 | effort_coeff: 12.0 10 | vel_coeff: 0.0048 11 | power_offset: 0 12 | twist_angular: 0.5233 13 | timeout: 0.1 14 | pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 15 | twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] 16 | 17 | # BalanceController 18 | imu_name: "base_imu" 19 | m_w: 0.72 20 | m: 11.48 21 | m_b: 1.29 22 | i_w: 0.01683 23 | l: 0.0587 24 | y_b: 0.16 25 | z_b: 0.0468 26 | g: 9.8 27 | i_m: 0.1982 28 | wheel_radius: 0.125 29 | wheel_base: 0.42 30 | left: 31 | wheel_joint: "left_wheel_joint" 32 | block_joint: "left_momentum_block_joint" 33 | right: 34 | wheel_joint: "right_wheel_joint" 35 | block_joint: "right_momentum_block_joint" 36 | 37 | q: [ 0.5, 0.5, 40, 51, 51, 10, 0.2, 0.1, 1, 1 ] 38 | r: [ 0.5, 0.5, 0.002, 0.002 ] 39 | -------------------------------------------------------------------------------- /rm_chassis_controllers/test/load_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | false 8 | 9 | 11 | 13 | 14 | 15 | 17 | 18 | -------------------------------------------------------------------------------- /rm_chassis_controllers/test/mecanum.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | chassis_controller: 3 | type: rm_chassis_controllers/OmniController 4 | # ChassisBase 5 | publish_rate: 100 6 | enable_odom_tf: true 7 | publish_odom_tf: false 8 | power: 9 | effort_coeff: 3.9 10 | vel_coeff: 0.00855 11 | power_offset: -9.8 12 | twist_angular: 0.5233 13 | timeout: 0.1 14 | pid_follow: { p: 5, i: 0, d: 0.8, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 15 | twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] 16 | 17 | # OmniController 18 | wheels: 19 | left_front: 20 | pose: [ 0.2, 0.185, 0. ] 21 | roller_angle: -0.785 22 | joint: left_front_wheel_joint 23 | <<: &wheel_setting 24 | radius: 0.07625 25 | pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 26 | right_front: 27 | pose: [ 0.2, -0.185, 0. ] 28 | roller_angle: 0.785 29 | joint: right_front_wheel_joint 30 | <<: *wheel_setting 31 | left_back: 32 | pose: [ -0.2, 0.185, 0. ] 33 | roller_angle: 0.785 34 | joint: left_back_wheel_joint 35 | <<: *wheel_setting 36 | right_back: 37 | pose: [ -0.2, -0.185, 0. ] 38 | roller_angle: -0.785 39 | joint: right_back_wheel_joint 40 | <<: *wheel_setting 41 | -------------------------------------------------------------------------------- /rm_chassis_controllers/test/omni.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | chassis_controller: 3 | type: rm_chassis_controllers/OmniController 4 | # ChassisBase 5 | publish_rate: 100 6 | enable_odom_tf: true 7 | power: 8 | effort_coeff: 12.0 9 | vel_coeff: 0.0048 10 | power_offset: 0 11 | twist_angular: 0.5233 12 | timeout: 0.1 13 | pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 14 | twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] 15 | 16 | # OmniController 17 | wheels: 18 | left_front: 19 | pose: [ 0.147, 0.147, 2.356 ] 20 | joint: left_front_wheel_joint 21 | <<: &wheel_setting 22 | roller_angle: 0. 23 | radius: 0.07625 24 | pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 25 | right_front: 26 | pose: [ 0.147, -0.147, 0.785 ] 27 | joint: right_front_wheel_joint 28 | <<: *wheel_setting 29 | left_back: 30 | pose: [ -0.147, 0.147, -2.356 ] 31 | joint: left_back_wheel_joint 32 | <<: *wheel_setting 33 | right_back: 34 | pose: [ -0.147, -0.147, -0.785 ] 35 | joint: right_back_wheel_joint 36 | <<: *wheel_setting 37 | -------------------------------------------------------------------------------- /rm_chassis_controllers/test/swerve.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | chassis_controller: 3 | # ChassisBase 4 | type: rm_chassis_controllers/SwerveController 5 | publish_rate: 100 6 | enable_odom_tf: true 7 | power: 8 | effort_coeff: 12.0 9 | vel_coeff: 0.0048 10 | power_offset: -3 11 | timeout: 0.1 12 | twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] 13 | pid_follow: { p: 5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 14 | # SwerveController 15 | modules: 16 | left_front: 17 | position: [ 0.175, 0.175 ] 18 | pivot: 19 | joint: left_front_pivot_joint 20 | <<: &pivot_setting 21 | offset: 0. 22 | pid: { p: 5, i: 50, d: 0, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } 23 | wheel: 24 | joint: left_front_wheel_joint 25 | <<: &wheel_setting 26 | radius: 0.049 27 | pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } 28 | right_front: 29 | position: [ 0.175, -0.175 ] 30 | pivot: 31 | joint: right_front_pivot_joint 32 | <<: *pivot_setting 33 | wheel: 34 | joint: right_front_wheel_joint 35 | <<: *wheel_setting 36 | left_back: 37 | position: [ -0.175, 0.175 ] 38 | pivot: 39 | joint: left_back_pivot_joint 40 | <<: *pivot_setting 41 | wheel: 42 | joint: left_back_wheel_joint 43 | <<: *wheel_setting 44 | right_back: 45 | position: [ -0.175, -0.175 ] 46 | pivot: 47 | joint: right_back_pivot_joint 48 | <<: *pivot_setting 49 | wheel: 50 | joint: right_back_wheel_joint 51 | <<: *wheel_setting 52 | -------------------------------------------------------------------------------- /rm_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rm_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 29 | 0.1.7 30 | * Contributors: ye-luo-xi-tui 31 | 32 | 0.1.7 (2022-09-10) 33 | ------------------ 34 | * Merge remote-tracking branch 'origin/master' 35 | * Contributors: qiayuan 36 | 37 | 0.1.6 (2022-06-16) 38 | ------------------ 39 | * Merge pull request `#76 `_ from Edwinlinks/tof-radar-controller 40 | Finished tof radar controller and delete tof sensor controller 41 | * Change exec depend tof_sensor_controller to tof_radar_controller 42 | * Merge remote-tracking branch 'origin/master' 43 | * Contributors: Edwinlinks, QiayuanLiao, qiayuan 44 | 45 | 0.1.5 (2022-06-10) 46 | ------------------ 47 | * Merge remote-tracking branch 'origin/master' 48 | * 0.1.4 49 | * Merge remote-tracking branch 'origin/master' 50 | * Merge pull request `#60 `_ from ye-luo-xi-tui/master 51 | Add subpackage to metapackage's package.xml 52 | * Merge branch 'orientation_controller' 53 | * Add all subpackages to metapackage. 54 | * Merge branch 'master' into gimbal_track 55 | * Merge pull request `#46 `_ from ye-luo-xi-tui/master 56 | Deprecated imu_filter_controller 57 | * Contributors: QiayuanLiao, YuuinIH, qiayuan, yezi 58 | 59 | 0.1.3 (2022-03-28) 60 | ------------------ 61 | * Deprecated imu_filter_controller(Since the update frequency of the control loop is not stable, some of 62 | the camera trigger signals of imu will be lost. We put the imu filter down to the hardware resource layer, so 63 | imu_extra_handle is breaking. ) 64 | * Contributors: qiayuan, ye-luo-xi-tui, yezi 65 | 66 | 0.1.2 (2022-01-08) 67 | ------------------ 68 | 69 | 0.1.1 (2021-08-12) 70 | ------------------ 71 | * Set all version to the same 72 | * Add license to robot_state_controller source files 73 | * Add rm_controllers 74 | * Contributors: qiayuan 75 | -------------------------------------------------------------------------------- /rm_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.3) 2 | project(rm_controllers) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /rm_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_controllers 4 | 0.1.11 5 | Meta package that contains package for RoboMaster. 6 | Qiayuan Liao 7 | 8 | BSD 9 | 10 | https://github.com/rm-controls/rm_controllers 11 | https://github.com/rm-controls/rm_controllers/issues 12 | 13 | Qiayuan Liao 14 | 15 | catkin 16 | gpio_controller 17 | mimic_joint_controller 18 | rm_calibration_controllers 19 | rm_chassis_controllers 20 | rm_gimbal_controllers 21 | rm_orientation_controller 22 | rm_shooter_controllers 23 | robot_state_controller 24 | tof_radar_controller 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rm_gimbal_controllers) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Find catkin macros and libraries 13 | 14 | find_package(catkin REQUIRED 15 | COMPONENTS 16 | roscpp 17 | 18 | rm_common 19 | 20 | controller_interface 21 | effort_controllers 22 | tf2_eigen 23 | tf2_geometry_msgs 24 | visualization_msgs 25 | dynamic_reconfigure 26 | angles 27 | ) 28 | 29 | generate_dynamic_reconfigure_options( 30 | cfg/BulletSolver.cfg 31 | cfg/GimbalBase.cfg 32 | ) 33 | 34 | ################################### 35 | ## catkin specific configuration ## 36 | ################################### 37 | ## The catkin_package macro generates cmake config files for your package 38 | ## Declare things to be passed to dependent projects 39 | ## INCLUDE_DIRS: uncomment this if your package contains header files 40 | ## LIBRARIES: libraries you create in this project that dependent projects also need 41 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 42 | ## DEPENDS: system dependencies of this project that dependent projects also need 43 | catkin_package( 44 | INCLUDE_DIRS 45 | include 46 | LIBRARIES 47 | CATKIN_DEPENDS 48 | roscpp 49 | 50 | rm_common 51 | 52 | effort_controllers 53 | tf2_eigen 54 | tf2_geometry_msgs 55 | visualization_msgs 56 | dynamic_reconfigure 57 | LIBRARIES ${PROJECT_NAME} 58 | ) 59 | 60 | ########### 61 | ## Build ## 62 | ########### 63 | 64 | ## Specify additional locations of header files 65 | include_directories( 66 | include 67 | ${catkin_INCLUDE_DIRS} 68 | ) 69 | 70 | ## Declare a cpp library 71 | add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp) 72 | 73 | ## Specify libraries to link executable targets against 74 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 75 | 76 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 77 | 78 | ############# 79 | ## Install ## 80 | ############# 81 | 82 | ## Mark executables and/or libraries for installation 83 | install( 84 | TARGETS ${PROJECT_NAME} 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 88 | ) 89 | 90 | # Mark cpp header files for installation 91 | install( 92 | DIRECTORY include/${PROJECT_NAME}/ 93 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 94 | FILES_MATCHING PATTERN "*.h" 95 | ) 96 | 97 | # Mark other files for installation 98 | install( 99 | DIRECTORY test 100 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 101 | ) 102 | install( 103 | FILES rm_gimbal_controllers_plugins.xml 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | 107 | ############# 108 | ## Testing ## 109 | ############# 110 | 111 | #if (${CATKIN_ENABLE_TESTING}) 112 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 113 | # ## Add gtest based cpp test target and link libraries 114 | # catkin_add_gtest(${PROJECT_NAME}-test 115 | # test/test_ros_package_template.cpp 116 | # test/AlgorithmTest.cpp) 117 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 118 | #endif () 119 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/README.md: -------------------------------------------------------------------------------- 1 | # rm_gimbal_controllers 2 | 3 | ## Overview 4 | 5 | The rm_gimbal_controllers has three states: RATE, TRACK, and DIRECT. It performs PID control on the yaw joint and pitch joint according to commands. It can also perform moving average filtering based on detection data and calculate, predict and track targets based on the ballistic model. 6 | 7 | **Keywords:** ROS, Robomaster, gimbal, bullet solver, moving average filter 8 | 9 | ### License 10 | 11 | The source code is released under a [ BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). 12 | 13 | **Author: DynamicX
14 | Affiliation: DynamicX
15 | Maintainer: DynamicX** 16 | 17 | The package has been tested under [ROS](https://www.ros.org/) Indigo, Melodic and Noetic on respectively Ubuntu 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 18 | 19 | ### Hardware interface type 20 | + `JointStateInterface` Used to get the speed and position of gimbal joint. 21 | + `EffortJointInterface` Used to send effort command to gimbal joint . 22 | + `RoboStateInterface` Used to get the current and historical transform between gimbal and the world coordinate system and the transform between visual target and the world coordinate system. 23 | 24 | ## Installation 25 | 26 | ### Installation from Packages 27 | 28 | To install all packages from the this repository as Debian packages use 29 | 30 | sudo apt-get install ros-noetic-rm-gimbal-controllers 31 | 32 | Or better, use `rosdep`: 33 | 34 | sudo rosdep install --from-paths src 35 | 36 | ### Dependencies 37 | * roscpp 38 | * rm_common 39 | * effort_controllers 40 | * tf2_eigen 41 | * tf2_geometry_msgs 42 | * visualization_msgs 43 | * dynamic_reconfigure 44 | 45 | ## ROS API 46 | 47 | #### Subscribed Topics 48 | * **`command`** (rm_msgs/GimbalCmd) 49 | 50 | Set gimbal mode, pitch and yaw axis rotation speed, tracking target, pointing target and coordinate system. 51 | 52 | * **`/detection`** (rm_msgs/TargetDetectionArray) 53 | 54 | Receive visual recognition data. 55 | 56 | * **`//camera_info`** (CameraInfo) 57 | 58 | Make sure that the detection node receives a new frame of image and sends the prediction data to the detection node. 59 | 60 | #### Published Topics 61 | * **`error`** (rm_msgs/GimbalDesError) 62 | The error calculated by the ballistic model to shoot at the current gimbal angle to the target. 63 | 64 | * **`track`** (rm_msgs/TrackDataArray) 65 | The predicted data used for detection node to decide the ROI. 66 | ##### Bullet solver 67 | * **`model_desire`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) 68 | Used to visualize the desired bullet trajectory. 69 | 70 | * **`model_real`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) 71 | Used to visualize the trajectory that caculated by ballistic model in the current gimbal angle. 72 | #### Parameters 73 | * **`detection_topic`** (string, default: "/detection") 74 | 75 | The name of the topic where detection node gets predicted data. 76 | 77 | * **`detection_frame`** (string, default: "detection") 78 | 79 | The name of the frame of detection. 80 | 81 | * **`camera_topic`** (string, default: "/galaxy_camera/camera_info") 82 | 83 | The name of the topic that is determined that the detection node receives a new frame of image and sends the prediction data to the detection node. 84 | 85 | * **`publish_rate`** (double) 86 | 87 | Frequency (in Hz) of publishing gimbal error. 88 | 89 | * **`chassis_angular_data_num`** (double) 90 | 91 | The number of angle data of chassis.Used for chassis angular average filter. 92 | 93 | * **`time_compensation`** (double) 94 | 95 | Time(in s) of image transmission delay(in s).Used to compensate for the effects of images transimission delay 96 | 97 | ##### Bullet solver 98 | _Bullet solver is used to get the bullet drop point_ 99 | * **`resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30`** ( `double` ) 100 | 101 | The air resistance coefficient used for bullet solver when bullet speed is 10 m/s, 15 m/s, 16 m/s, 18 m/s and 30 m/s. 102 | 103 | * **`g`** (double) 104 | 105 | Acceleration of gravity. 106 | 107 | * **`delay`** (double) 108 | 109 | Shooting delay time(in s) after shooter get the shooting command.Used to compensate for the effects of launch delay. 110 | 111 | * **`timeout`** (double) 112 | 113 | Timeout time((in s)) of bullet solver.Used to judge whether bullet solver can caculate the bullet drop point. 114 | 115 | ##### Moving average filter 116 | _Moving average filter is used for filter the target armor center when target is spin._ 117 | * **`is_debug`** ( `bool`, default: true ) 118 | 119 | The debug status.When it is true, debug data will be pulished on the filter topic. 120 | 121 | * **`pos_data_num`** (double, default: 20) 122 | 123 | The number of armor position data. 124 | 125 | * **`vel_data_num`** (double, default: 30) 126 | 127 | The number of armor linear velocity data. 128 | 129 | * **`gyro_data_num`** (double, default: 100) 130 | 131 | The number of target rotation speed data. 132 | 133 | * **`center_data_num`** (double, default: 50) 134 | 135 | The number of target rotation center position data. 136 | 137 | * **`center_offset_z`** (double) 138 | 139 | Offset(in meter) on the z axis.Used to compensate for the error of filter result on z axis. 140 | 141 | ## Controller configuration examples 142 | 143 | ``` 144 | gimbal_controller: 145 | type: rm_gimbal_controllers/Controller 146 | time_compensation: 0.03 147 | publish_rate: 100 148 | chassis_angular_data_num: 20 149 | camera_topic: "/galaxy_camera/camera_info" 150 | yaw: 151 | joint: "yaw_joint" 152 | pid: { p: 8, i: 0, d: 0.4, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } 153 | pitch: 154 | joint: "pitch_joint" 155 | pid: { p: 10, i: 50, d: 0.3, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } 156 | bullet_solver: 157 | resistance_coff_qd_10: 0.45 158 | resistance_coff_qd_15: 0.1 159 | resistance_coff_qd_16: 0.7 160 | resistance_coff_qd_18: 0.55 161 | resistance_coff_qd_30: 3.0 162 | g: 9.81 163 | delay: 0.1 164 | dt: 0.001 165 | timeout: 0.001 166 | publish_rate: 50 167 | moving_average_filter: 168 | is_debug: true 169 | center_offset_z: 0.05 170 | pos_data_num: 20 171 | vel_data_num: 30 172 | center_data_num: 50 173 | gyro_data_num: 100 174 | ``` 175 | 176 | 177 | ## Bugs & Feature Requests 178 | 179 | Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). 180 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/cfg/BulletSolver.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rm_gimbal_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("resistance_coff_qd_10", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) 9 | gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) 10 | gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) 11 | gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) 12 | gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) 13 | gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) 14 | gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) 15 | gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.105, 0.0, 0.5) 16 | gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5) 17 | gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) 18 | gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) 19 | gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) 20 | gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) 21 | gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) 22 | gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) 23 | gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) 24 | gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0.0, 4.5, 20) 25 | gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0) 26 | gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) 27 | gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) 28 | 29 | exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) 30 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/cfg/GimbalBase.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rm_gimbal_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) 9 | gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) 10 | gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0) 11 | gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0) 12 | gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) 13 | gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) 14 | gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) 15 | gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) 16 | 17 | exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 18 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 8/14/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | namespace rm_gimbal_controllers 55 | { 56 | struct Config 57 | { 58 | double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, 59 | resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, 60 | ban_shoot_duration, gimbal_switch_duration, max_switch_angle, min_switch_angle, min_shoot_beforehand_vel, 61 | max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; 62 | int min_fit_switch_count; 63 | }; 64 | 65 | class BulletSolver 66 | { 67 | public: 68 | explicit BulletSolver(ros::NodeHandle& controller_nh); 69 | 70 | bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, 71 | double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z); 72 | double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, 73 | double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); 74 | double getResistanceCoefficient(double bullet_speed) const; 75 | double getYaw() const 76 | { 77 | return output_yaw_; 78 | } 79 | double getPitch() const 80 | { 81 | return -output_pitch_; 82 | } 83 | void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, 84 | geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, 85 | double r1, double r2, double dz, int armors_num); 86 | void judgeShootBeforehand(const ros::Time& time, double v_yaw); 87 | void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); 88 | void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); 89 | void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); 90 | ~BulletSolver() = default; 91 | 92 | private: 93 | std::shared_ptr> path_desire_pub_; 94 | std::shared_ptr> path_real_pub_; 95 | std::shared_ptr> shoot_beforehand_cmd_pub_; 96 | std::shared_ptr> fly_time_pub_; 97 | ros::Subscriber identified_target_change_sub_; 98 | ros::Time switch_armor_time_{}; 99 | realtime_tools::RealtimeBuffer config_rt_buffer_; 100 | dynamic_reconfigure::Server* d_srv_{}; 101 | Config config_{}; 102 | double max_track_target_vel_; 103 | double output_yaw_{}, output_pitch_{}; 104 | double bullet_speed_{}, resistance_coff_{}; 105 | double fly_time_; 106 | double switch_hysteresis_; 107 | int shoot_beforehand_cmd_{}; 108 | int selected_armor_; 109 | int count_; 110 | bool track_target_ = true; 111 | bool identified_target_change_ = true; 112 | bool is_in_delay_before_switch_{}; 113 | bool dynamic_reconfig_initialized_{}; 114 | 115 | geometry_msgs::Point target_pos_{}; 116 | visualization_msgs::Marker marker_desire_; 117 | visualization_msgs::Marker marker_real_; 118 | }; 119 | } // namespace rm_gimbal_controllers 120 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_gimbal_controllers 4 | 0.1.11 5 | RoboMaster standard robot Gimbal controller 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | 15 | rm_common 16 | 17 | controller_interface 18 | effort_controllers 19 | tf2_eigen 20 | tf2_geometry_msgs 21 | visualization_msgs 22 | dynamic_reconfigure 23 | angles 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | The Controller is RoboMaster robot Gimbal controller. It expects a EffortJointInterface type of hardware 7 | interface. 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/test/gimbal_config_template.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | robot_state_controller: 3 | type: robot_state_controller/RobotStateController 4 | publish_rate: 50 5 | joint_state_controller: 6 | type: joint_state_controller/JointStateController 7 | publish_rate: 50 8 | gimbal_controller: 9 | type: rm_gimbal_controllers/Controller 10 | yaw: 11 | joint: "yaw_joint" 12 | pid: { p: 7.5, i: 0, d: 0.3, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } 13 | k_chassis_vel: 0.01 14 | pitch: 15 | joint: "pitch_joint" 16 | pid: { p: 22.0, i: 0, d: 0.75, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } 17 | imu_name: "gimbal_imu" 18 | feedforward: 19 | gravity: 0.0 20 | enable_gravity_compensation: false 21 | mass_origin: [ 0.0,0.0,0.0 ] 22 | bullet_solver: 23 | resistance_coff_qd_10: 0.45 24 | resistance_coff_qd_15: 1.0 25 | resistance_coff_qd_16: 0.7 26 | resistance_coff_qd_18: 0.55 27 | resistance_coff_qd_30: 5.0 28 | g: 9.81 29 | delay: 0.15 30 | dt: 0.001 31 | timeout: 0.001 32 | publish_rate: 50 33 | -------------------------------------------------------------------------------- /rm_gimbal_controllers/test/load_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 11 | 13 | 14 | -------------------------------------------------------------------------------- /rm_orientation_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rm_orientation_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#91 `_ from ye-luo-xi-tui/imu_offline 29 | Dealing with the situation of imu offline. 30 | * Dealing with the situation of imu offline. 31 | * Merge pull request `#86 `_ from NaHCO3bc/Readme 32 | Fix the dependence part bug. 33 | * Modify the test file folder. 34 | * Fix the dependence part bug. 35 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 36 | 0.1.7 37 | * Contributors: NaHCO3bc, ye-luo-xi-tui, yezi 38 | 39 | 0.1.7 (2022-09-10) 40 | ------------------ 41 | * Optimize rm_orientation_controller. 42 | * Merge remote-tracking branch 'origin/master' 43 | * Contributors: qiayuan, yezi 44 | 45 | 0.1.6 (2022-06-16) 46 | ------------------ 47 | * Merge remote-tracking branch 'origin/master' 48 | * Contributors: qiayuan 49 | 50 | 0.1.5 (2022-06-10) 51 | ------------------ 52 | * Merge remote-tracking branch 'origin/master' 53 | * Merge pull request `#63 `_ from ye-luo-xi-tui/master 54 | Publish tf from odom to base_link use imu data which is got from interface until getting imu data from topic 55 | * 0.1.4 56 | * Publish tf from odom to base_link use imu data which is got from interface until get imu data from topic. 57 | * Merge pull request `#53 `_ from ye-luo-xi-tui/orientation_controller 58 | Fix a bug that time stamp error 59 | * Fix a bug that time stamp error when it can't find transform from imu to odom. 60 | * Merge branch 'master' into gimbal_track 61 | * Merge pull request `#45 `_ from mlione/master 62 | Delete some config files in rm_controllers. 63 | * Merge pull request `#50 `_ from ye-luo-xi-tui/ori 64 | Make rm_orientation_controller publish tf use imu data on the topic 65 | * Delete some config files in rm_controller. 66 | * Solve a problem that look up future transform. 67 | * Delete publish_rate and relative codes. 68 | * Publish tf use imu data on the topic. 69 | * Merge pull request `#46 `_ from ye-luo-xi-tui/master 70 | Deprecated imu_filter_controller 71 | * Contributors: QiayuanLiao, YuuinIH, mlione, qiayuan, yezi 72 | 73 | 0.1.3 (2022-03-28) 74 | ------------------ 75 | * Merge branch 'master' into forward_feed 76 | * Merge pull request `#40 `_ from ye-luo-xi-tui/maintain 77 | Delete configuration of robot_state_controller in each of controllers' config file 78 | * Delete configuration of robot_state_controller in each of controllers' config file 79 | * Merge branch 'master' into standard3 80 | * Merge remote-tracking branch 'origin/master' 81 | * Contributors: QiayuanLiao, qiayuan, ye-luo-xi-tui, yezi 82 | 83 | 0.1.2 (2022-01-08) 84 | ------------------ 85 | * Merge branch 'master' into omni_wheel_controller 86 | * Merge pull request `#23 `_ from ye-luo-xi-tui/master 87 | Add ImuFilterBase, overwrite ComplementaryController, add MadgwickController 88 | * Fix format error 89 | (cherry picked from commit f7519e6800095d933f9d4c85f108ae5260001572) 90 | * Merge branch 'master' of https://github.com/YuuinIH/rm_controllers 91 | * Merge branch 'gimbal/opti_or_simplify' into chassis/balance_imu_interface 92 | * Run pre-commit 93 | * Merge branch 'master' into gimbal/opti_or_simplify 94 | # Conflicts: 95 | # rm_orientation_controller/launch/load_controllers.launch 96 | # rm_orientation_controller/src/orientation_controller.cpp 97 | * Merge branch 'master' into chassis/balance_imu_interface 98 | * Run pre-commit 99 | * Update the config of rm_orientation_controller, load only one controller on launch instead of spawn controllers 100 | * Correct "rm_orientation_controllers" to "rm_orientation_controller". 101 | * Fix pre-commit 102 | * Code style and delete unnecessary 103 | * Merge branch 'master' into imu_filter_controllers 104 | # Conflicts: 105 | # imu_filter_controllers/CMakeLists.txt 106 | # imu_filter_controllers/include/imu_filter_controllers/complementary_controller.h 107 | # imu_filter_controllers/package.xml 108 | # imu_filter_controllers/src/complementary_controller.cpp 109 | * Upgrade "hardware_interface::RobotStateInterface" to "rm_control::RobotStateInterface". 110 | * Update orientation controller version. 111 | * Merge remote-tracking branch 'alias_memory/metapackage' 112 | * Move all files to rm_orientation_controller/rm_orientation_controller,prepare for merge 113 | * Contributors: BruceLannn, QiayuanLiao, YuuinIH, qiayuan, yezi 114 | -------------------------------------------------------------------------------- /rm_orientation_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rm_orientation_controller) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | roscpp 16 | 17 | sensor_msgs 18 | rm_common 19 | 20 | pluginlib 21 | controller_interface 22 | forward_command_controller 23 | tf2_geometry_msgs 24 | angles 25 | ) 26 | 27 | ## Find system libraries 28 | find_package(Eigen3 REQUIRED) 29 | find_package(Boost REQUIRED) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS 33 | include 34 | LIBRARIES 35 | CATKIN_DEPENDS 36 | roscpp 37 | 38 | sensor_msgs 39 | rm_common 40 | 41 | pluginlib 42 | forward_command_controller 43 | tf2_geometry_msgs 44 | angles 45 | LIBRARIES ${PROJECT_NAME} 46 | ) 47 | 48 | 49 | ########### 50 | ## Build ## 51 | ########### 52 | 53 | ## Specify additional locations of header files 54 | ## Your package locations should be listed before other locations 55 | include_directories( 56 | include 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | 60 | ## Declare a cpp library 61 | add_library(${PROJECT_NAME} src/orientation_controller.cpp) 62 | 63 | ## Specify libraries to link executable targets against 64 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 65 | 66 | ############# 67 | ## Install ## 68 | ############# 69 | 70 | # Mark executables and/or libraries for installation 71 | install( 72 | TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 73 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 74 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 76 | ) 77 | 78 | # Mark cpp header files for installation 79 | install( 80 | DIRECTORY include/${PROJECT_NAME}/ 81 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 82 | FILES_MATCHING PATTERN "*.h" 83 | ) 84 | 85 | # Mark other files for installation 86 | install( 87 | DIRECTORY test 88 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 89 | ) 90 | install( 91 | FILES rm_orientation_controller_plugin.xml 92 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 93 | ) 94 | 95 | ############# 96 | ## Testing ## 97 | ############# 98 | 99 | #if (${CATKIN_ENABLE_TESTING}) 100 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 101 | # ## Add gtest based cpp test target and link libraries 102 | # catkin_add_gtest(${PROJECT_NAME}-test 103 | # test/test_ros_package_template.cpp 104 | # test/AlgorithmTest.cpp) 105 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 106 | #endif () 107 | -------------------------------------------------------------------------------- /rm_orientation_controller/README.md: -------------------------------------------------------------------------------- 1 | # Package Name 2 | 3 | ## Overview 4 | 5 | This is a template: replace, remove, and add where required. Describe here what this package does and what it's meant 6 | for in a few sentences. 7 | 8 | **Keywords:** example, package, template 9 | 10 | Or, add some keywords to the Bitbucket or GitHub repository. 11 | 12 | ### License 13 | 14 | The source code is released under a [BSD 3-Clause license](LICENSE). 15 | 16 | **Author: Péter Fankhauser
17 | Affiliation: [ANYbotics](https://www.anybotics.com/)
18 | Maintainer: Péter Fankhauser, pfankhauser@anybotics.com** 19 | 20 | The PACKAGE NAME package has been tested under [ROS] Indigo, Melodic and Noetic on respectively Ubuntu 14.04, 18.04 and 21 | 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 22 | 23 | [![Build Status](http://rsl-ci.ethz.ch/buildStatus/icon?job=ros_best_practices)](http://rsl-ci.ethz.ch/job/ros_best_practices/) 24 | 25 | ![Example image](doc/example.jpg) 26 | 27 | [comment]: <> (### Publications) 28 | 29 | [comment]: <> (If you use this work in an academic context, please cite the following publication(s):) 30 | 31 | [comment]: <> (* P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, and R. Siegwart: **PAPER TITLE**. IEEE/RSJ International Conference) 32 | 33 | [comment]: <> ( on Intelligent Robots and Systems (IROS), 2015. ([PDF](http://dx.doi.org/10.3929/ethz-a-010173654))) 34 | 35 | [comment]: <> ( @inproceedings{Fankhauser2015,) 36 | 37 | [comment]: <> ( author = {Fankhauser, P\'{e}ter and Hutter, Marco},) 38 | 39 | [comment]: <> ( booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},) 40 | 41 | [comment]: <> ( title = {{PAPER TITLE}},) 42 | 43 | [comment]: <> ( publisher = {IEEE},) 44 | 45 | [comment]: <> ( year = {2015}) 46 | 47 | [comment]: <> ( }) 48 | 49 | ## Installation 50 | 51 | ### Installation from Packages 52 | 53 | To install all packages from the this repository as Debian packages use 54 | 55 | sudo apt-get install ros-noetic-... 56 | 57 | Or better, use `rosdep`: 58 | 59 | sudo rosdep install --from-paths src 60 | 61 | ### Dependencies 62 | 63 | - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), 64 | - [Eigen] (linear algebra library) 65 | 66 | sudo rosdep install --from-paths src 67 | 68 | ### Running in Docker 69 | 70 | Docker is a great way to run an application with all dependencies and libraries bundles together. Make sure 71 | to [install Docker](https://docs.docker.com/get-docker/) first. 72 | 73 | First, spin up a simple container: 74 | 75 | docker run -ti --rm --name ros-container ros:noetic bash 76 | 77 | This downloads the `ros:noetic` image from the Docker Hub, indicates that it requires an interactive terminal (`-t, -i`) 78 | , gives it a name (`--name`), removes it after you exit the container (`--rm`) and runs a command (`bash`). 79 | 80 | Now, create a catkin workspace, clone the package, build it, done! 81 | 82 | apt-get update && apt-get install -y git 83 | mkdir -p /ws/src && cd /ws/src 84 | git clone https://github.com/leggedrobotics/ros_best_practices.git 85 | cd .. 86 | rosdep install --from-path src 87 | catkin_make 88 | source devel/setup.bash 89 | roslaunch ros_package_template ros_package_template.launch 90 | 91 | ### Unit Tests 92 | 93 | Run the unit tests with 94 | 95 | catkin_make run_tests_ros_package_template 96 | 97 | ### Static code analysis 98 | 99 | Run the static code analysis with 100 | 101 | catkin_make roslint_ros_package_template 102 | 103 | ## Config files 104 | 105 | Config file folder/set 1 106 | 107 | * **config_file_1.yaml** Shortly explain the content of this config file 108 | 109 | Config file folder/set 2 110 | 111 | * **...** 112 | 113 | ## Nodes 114 | 115 | ### ros_package_template 116 | 117 | Reads temperature measurements and computed the average. 118 | 119 | #### Subscribed Topics 120 | 121 | * **`/temperature`** ([sensor_msgs/Temperature]) 122 | 123 | The temperature measurements from which the average is computed. 124 | 125 | #### Published Topics 126 | 127 | ... 128 | 129 | #### Services 130 | 131 | * **`get_average`** ([std_srvs/Trigger]) 132 | 133 | Returns information about the current average. For example, you can trigger the computation from the console with 134 | 135 | rosservice call /ros_package_template/get_average 136 | 137 | #### Parameters 138 | 139 | * **`subscriber_topic`** (string, default: "/temperature") 140 | 141 | The name of the input topic. 142 | 143 | * **`cache_size`** (int, default: 200, min: 0, max: 1000) 144 | 145 | The size of the cache. 146 | 147 | ### NODE_B_NAME 148 | 149 | ... 150 | 151 | #### Complete description 152 | 153 | ```yaml 154 | orientation_controller: 155 | type: rm_orientation_controller/Controller 156 | publish_rate: 100 157 | name: "gimbal_imu" 158 | frame_source: "odom" 159 | frame_target: "base_link" 160 | ``` 161 | 162 | ## Bugs & Feature Requests 163 | 164 | Please report bugs and request features using the [Issue Tracker](https://github.com/gdut-dynamic-x/rm_template/issues) 165 | . 166 | 167 | 168 | [ROS]: http://www.ros.org 169 | 170 | [rviz]: http://wiki.ros.org/rviz 171 | 172 | [Eigen]: http://eigen.tuxfamily.org 173 | 174 | [std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html 175 | 176 | [sensor_msgs/Temperature]: http://docs.ros.org/api/sensor_msgs/html/msg/Temperature.html 177 | -------------------------------------------------------------------------------- /rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bruce on 2021/5/19. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace rm_orientation_controller 15 | { 16 | class Controller : public controller_interface::MultiInterfaceController 18 | { 19 | public: 20 | Controller() = default; 21 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 22 | void update(const ros::Time& time, const ros::Duration& period) override; 23 | 24 | private: 25 | bool getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, 26 | const double y, const double z, const double w); 27 | void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg); 28 | 29 | rm_control::RmImuSensorHandle imu_sensor_; 30 | rm_control::RobotStateHandle robot_state_; 31 | 32 | ros::Time last_imu_update_time_; 33 | 34 | rm_common::TfRtBroadcaster tf_broadcaster_{}; 35 | geometry_msgs::TransformStamped source2target_msg_; 36 | 37 | std::string frame_source_; 38 | std::string frame_target_; 39 | 40 | ros::Subscriber imu_data_sub_; 41 | bool receive_imu_msg_ = false; 42 | }; 43 | } // namespace rm_orientation_controller 44 | -------------------------------------------------------------------------------- /rm_orientation_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_orientation_controller 4 | 0.1.11 5 | RoboMaster standard robot orientation controller 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | 15 | sensor_msgs 16 | rm_common 17 | 18 | pluginlib 19 | controller_interface 20 | forward_command_controller 21 | tf2_geometry_msgs 22 | angles 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /rm_orientation_controller/rm_orientation_controller_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | The Controller is RoboMaster robot Orientation controller. It expects a EffortJointInterface type of 7 | hardware 8 | interface. 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /rm_orientation_controller/src/orientation_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bruce on 2021/5/19. 3 | // 4 | 5 | #include "rm_orientation_controller/orientation_controller.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace rm_orientation_controller 11 | { 12 | bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) 13 | { 14 | std::string name; 15 | if (!controller_nh.getParam("name", name) || !controller_nh.getParam("frame_source", frame_source_) || 16 | !controller_nh.getParam("frame_target", frame_target_)) 17 | { 18 | ROS_ERROR("Some params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); 19 | return false; 20 | } 21 | imu_sensor_ = robot_hw->get()->getHandle(name); 22 | robot_state_ = robot_hw->get()->getHandle("robot_state"); 23 | 24 | tf_broadcaster_.init(root_nh); 25 | imu_data_sub_ = root_nh.subscribe("data", 1, &Controller::imuDataCallback, this); 26 | source2target_msg_.header.frame_id = frame_source_; 27 | source2target_msg_.child_frame_id = frame_target_; 28 | source2target_msg_.transform.rotation.w = 1.0; 29 | return true; 30 | } 31 | 32 | void Controller::update(const ros::Time& time, const ros::Duration& period) 33 | { 34 | if (imu_sensor_.getTimeStamp() > last_imu_update_time_) 35 | { 36 | last_imu_update_time_ = imu_sensor_.getTimeStamp(); 37 | geometry_msgs::TransformStamped source2target; 38 | source2target.header.stamp = time; 39 | source2target.header.stamp.nsec += 1; // Avoid redundant timestamp 40 | source2target_msg_.header.stamp = time; 41 | source2target_msg_.header.stamp.nsec += 1; 42 | source2target_msg_ = 43 | getTransform(ros::Time(0), source2target, imu_sensor_.getOrientation()[0], imu_sensor_.getOrientation()[1], 44 | imu_sensor_.getOrientation()[2], imu_sensor_.getOrientation()[3]) ? 45 | source2target : 46 | source2target_msg_; 47 | robot_state_.setTransform(source2target_msg_, "rm_orientation_controller"); 48 | if (!receive_imu_msg_) 49 | tf_broadcaster_.sendTransform(source2target_msg_); 50 | } 51 | } 52 | 53 | bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, 54 | const double y, const double z, const double w) 55 | { 56 | source2target.header.frame_id = frame_source_; 57 | source2target.child_frame_id = frame_target_; 58 | source2target.transform.rotation.w = 1.0; 59 | tf2::Transform source2odom, odom2fixed, fixed2target; 60 | try 61 | { 62 | geometry_msgs::TransformStamped tf_msg; 63 | tf_msg = robot_state_.lookupTransform(frame_source_, "odom", time); 64 | tf2::fromMsg(tf_msg.transform, source2odom); 65 | tf_msg = robot_state_.lookupTransform("odom", imu_sensor_.getFrameId(), time); 66 | tf2::fromMsg(tf_msg.transform, odom2fixed); 67 | tf_msg = robot_state_.lookupTransform(imu_sensor_.getFrameId(), frame_target_, time); 68 | tf2::fromMsg(tf_msg.transform, fixed2target); 69 | } 70 | catch (tf2::TransformException& ex) 71 | { 72 | ROS_WARN("%s", ex.what()); 73 | return false; 74 | } 75 | tf2::Quaternion odom2fixed_quat; 76 | odom2fixed_quat.setValue(x, y, z, w); 77 | odom2fixed.setRotation(odom2fixed_quat); 78 | source2target.transform = tf2::toMsg(source2odom * odom2fixed * fixed2target); 79 | return true; 80 | } 81 | 82 | void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) 83 | { 84 | if (!receive_imu_msg_) 85 | receive_imu_msg_ = true; 86 | geometry_msgs::TransformStamped source2target; 87 | source2target.header.stamp = msg->header.stamp; 88 | getTransform(ros::Time(0), source2target, msg->orientation.x, msg->orientation.y, msg->orientation.z, 89 | msg->orientation.w); 90 | tf_broadcaster_.sendTransform(source2target); 91 | } 92 | 93 | } // namespace rm_orientation_controller 94 | 95 | PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase) 96 | -------------------------------------------------------------------------------- /rm_orientation_controller/test/load_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /rm_orientation_controller/test/orientation_config_template.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | orientation_controller: 3 | type: rm_orientation_controller/Controller 4 | publish_rate: 100 5 | name: "gimbal_imu" 6 | frame_source: "odom" 7 | frame_target: "base_link" 8 | -------------------------------------------------------------------------------- /rm_shooter_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rm_shooter_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge pull request `#129 `_ from ye-luo-xi-tui/master 8 | Shooter_controller would not check block when friction wheel don't rotate 9 | * Shooter_controller would not check block when friction wheel don't rotate. 10 | * Merge branch 'master' into dev/balance 11 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 12 | 0.1.10 13 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 14 | * Contributors: 1moule, ye-luo-xi-tui, yezi, yuchen 15 | 16 | 0.1.10 (2023-03-25) 17 | ------------------- 18 | * Merge pull request `#118 `_ from ye-luo-xi-tui/master 19 | Publish shoot state 20 | * Publish shoot state. 21 | * Merge pull request `#109 `_ from ye-luo-xi-tui/master 22 | Modifier default value of forward_push_threshold and exit_push_threshold 23 | * Modifier default value of forward_push_threshold and exit_push_threshold. 24 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 25 | 0.1.9 26 | * Contributors: ye-luo-xi-tui, yezi 27 | 28 | 0.1.9 (2023-02-21) 29 | ------------------ 30 | * Merge branch 'master' into balance_standard 31 | * Merge remote-tracking branch 'origin/fix_return' into fix_return 32 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 33 | 0.1.8 34 | * Merge branch 'rm-controls:master' into fix_return 35 | * Contributors: L-SY, lsy, ye-luo-xi-tui, yezi 36 | 37 | 0.1.8 (2022-11-24) 38 | ------------------ 39 | * Merge pull request `#93 `_ from NaHCO3bc/master 40 | Fix the bug that the shooter cannot be turned from push to ready 41 | * Modify the name and the description of the params about push forward threshold. 42 | * Optimize the logic of entering the block mode. 43 | * Fix the bug that shooter cannot push or enter block when the position error is too big. 44 | * Modify the params name. 45 | * Modify the params about enter and exit push mode. 46 | * Parametric position difference of trigger. 47 | * Fix the bug that the shooter cannot be turned from push to ready. 48 | * Merge pull request `#86 `_ from NaHCO3bc/Readme 49 | Fix the dependence part bug. 50 | * Modify the test file folder. 51 | * Fix the dependence part bug. 52 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 53 | 0.1.7 54 | * Contributors: NaHCO3bc, ye-luo-xi-tui 55 | 56 | 0.1.7 (2022-09-10) 57 | ------------------ 58 | * Try two fix double shoot 59 | * Merge remote-tracking branch 'origin/master' 60 | * Fix bug of shooting if statement 61 | * Contributors: qiayuan 62 | 63 | 0.1.6 (2022-06-16) 64 | ------------------ 65 | * Merge remote-tracking branch 'origin/master' 66 | * Contributors: qiayuan 67 | 68 | 0.1.5 (2022-06-10) 69 | ------------------ 70 | * Add hz counting in trigger test mode. 71 | * Merge remote-tracking branch 'origin/master' 72 | * Add testing option to shooter for testing the trigger without friction wheel 73 | * 0.1.4 74 | * Merge branch 'master' into gimbal_track 75 | * Merge pull request `#45 `_ from mlione/master 76 | Delete some config files in rm_controllers. 77 | * Delete some config files in rm_controller. 78 | * Merge pull request `#46 `_ from ye-luo-xi-tui/master 79 | Deprecated imu_filter_controller 80 | * Contributors: BruceLannn, QiayuanLiao, YuuinIH, mlione, qiayuan, yezi 81 | 82 | 0.1.3 (2022-03-28) 83 | ------------------ 84 | * Merge branch 'master' into forward_feed 85 | * Merge pull request `#40 `_ from ye-luo-xi-tui/maintain 86 | Delete configuration of robot_state_controller in each of controllers' config file 87 | * Merge branch 'master' into maintain 88 | # Conflicts: 89 | # rm_chassis_controllers/config/standard3.yaml 90 | * Merge pull request `#41 `_ from ye-luo-xi-tui/standard3 91 | Update standard3 config 92 | * Delete configuration of robot_state_controller in each of controllers' config file 93 | * Merge branch 'master' into standard3 94 | * Update standard3 config 95 | * Merge remote-tracking branch 'origin/master' 96 | * Contributors: QiayuanLiao, qiayuan, ye-luo-xi-tui, yezi 97 | 98 | 0.1.2 (2022-01-08) 99 | ------------------ 100 | * Merge branch 'master' of https://github.com/YuuinIH/rm_controllers 101 | * Merge branch 'gimbal/opti_or_simplify' into chassis/balance_imu_interface 102 | * Update the config of rm_shooter_controller, load only one controller on launch instead of spawn controllers 103 | * Merge branch 'master' into chassis/fix_filter 104 | * Merge remote-tracking branch 'origin/master' 105 | * Merge branch 'namespace' 106 | # Conflicts: 107 | # rm_chassis_controllers/README.md 108 | * Merge pull request `#15 `_ from ye-luo-xi-tui/namespace 109 | Change name of namespace:from hardware_interface to rm_control 110 | * Change name of namespace:from hardware_interface to rm_control. 111 | * Merge pull request `#8 `_ from Edwinlinks/master 112 | Update README.md in rm_shooter_controller 113 | * Update the format of Bugs & Feature Requests README.md in rm_shooter_controller 114 | * Fix LICENSE url 115 | * Update the format of Bugs & Feature Requests README.md in rm_shooter_controller 116 | * Update the README.md in rm_shooter_controller 117 | * Use “pragma once” in rm_shooter_controllers headers instead of include guards. 118 | * Correct dependencies and interface type. 119 | * Correct some shooter param's description. 120 | * Simplify shooter README.md. 121 | * Update shooter param's description. 122 | * Correct readme format. 123 | * Correct readme format. 124 | * Update controllers README. 125 | * Merge remote-tracking branch 'origin/master' 126 | * Update README.md 127 | * Delet a space bar. 128 | Delet space bar after sudo rosdep install --from-paths src 129 | * Add some param's description 130 | * Update shooter controller's README.md 131 | * Fix wrong naming "include/rm_shooter_controller" 132 | * Run pre-commit 133 | * Format rm_shooter_controllers using clang-format 134 | * Contributors: BruceLannn, QiayuanLiao, YuuinIH, chenzheng, link Edwin, qiayuan, yezi, 沐 135 | 136 | 0.1.1 (2021-08-12) 137 | ------------------ 138 | * Set all version to the same 139 | * Add license to rm_chassis_controllers and rm_gimbal_controllers source files 140 | * Merge remote-tracking branch 'alias_memory/metapackage' 141 | * Move all files to rm_shooter_controllers/rm_shooter_controllers, prepare for merge 142 | * Contributors: qiayuan 143 | -------------------------------------------------------------------------------- /rm_shooter_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rm_shooter_controllers) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | roscpp 16 | 17 | rm_common 18 | 19 | controller_interface 20 | effort_controllers 21 | dynamic_reconfigure 22 | ) 23 | 24 | generate_dynamic_reconfigure_options( 25 | cfg/Shooter.cfg 26 | ) 27 | 28 | ################################### 29 | ## catkin specific configuration ## 30 | ################################### 31 | ## The catkin_package macro generates cmake config files for your package 32 | ## Declare things to be passed to dependent projects 33 | ## INCLUDE_DIRS: uncomment this if your package contains header files 34 | ## LIBRARIES: libraries you create in this project that dependent projects also need 35 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 36 | ## DEPENDS: system dependencies of this project that dependent projects also need 37 | catkin_package( 38 | INCLUDE_DIRS 39 | include 40 | LIBRARIES 41 | CATKIN_DEPENDS 42 | roscpp 43 | 44 | rm_common 45 | 46 | effort_controllers 47 | dynamic_reconfigure 48 | LIBRARIES ${PROJECT_NAME} 49 | ) 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | ## Specify additional locations of header files 56 | ## Your package locations should be listed before other locations 57 | include_directories( 58 | include 59 | ${catkin_INCLUDE_DIRS} 60 | ) 61 | 62 | ## Declare a cpp library 63 | add_library(${PROJECT_NAME} 64 | src/standard.cpp) 65 | 66 | ## Specify libraries to link executable targets against 67 | target_link_libraries(${PROJECT_NAME} 68 | ${catkin_LIBRARIES}) 69 | 70 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 71 | 72 | ############# 73 | ## Install ## 74 | ############# 75 | 76 | # Mark executables and/or libraries for installation 77 | install( 78 | TARGETS ${PROJECT_NAME} 79 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 80 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 81 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 82 | ) 83 | 84 | # Mark cpp header files for installation 85 | install( 86 | DIRECTORY include/${PROJECT_NAME}/ 87 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 88 | FILES_MATCHING PATTERN "*.h" 89 | ) 90 | 91 | # Mark other files for installation 92 | install( 93 | DIRECTORY test 94 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 95 | ) 96 | install( 97 | FILES rm_shooter_controllers_plugins.xml 98 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 99 | ) 100 | 101 | ############# 102 | ## Testing ## 103 | ############# 104 | 105 | #if (${CATKIN_ENABLE_TESTING}) 106 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 107 | # ## Add gtest based cpp test target and link libraries 108 | # catkin_add_gtest(${PROJECT_NAME}-test 109 | # test/test_ros_package_template.cpp 110 | # test/AlgorithmTest.cpp) 111 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 112 | #endif () 113 | -------------------------------------------------------------------------------- /rm_shooter_controllers/README.md: -------------------------------------------------------------------------------- 1 | # rm_shooter_controller 2 | 3 | ## Overview 4 | 5 | The rm_shooter_controller has four states: STOP, READY, PUSH, and BLOCK, it controls the left and right friction wheels and the trigger wheel through PID algorithm according to the command. It can set the bullet speed by setting the angular velocity of the friction wheel, and at the same time realizes the block detection. 6 | 7 | **Keywords:** ROS, robomaster, shooter 8 | 9 | #### License 10 | 11 | The source code is released under a [BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). 12 | 13 | **Author: DynamicX
14 | Affiliation: DynamicX
15 | Maintainer: DynamicX** 16 | 17 | The rm_shooter_controller package has been tested under [ROS](http://www.ros.org) Melodic and Noetic on respectively 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 18 | 19 | ### Hardware interface type 20 | 21 | + `JointStateInterface` Used to obtain the speed of friction wheel and trigger wheel and the position of trigger wheel. 22 | + `EffortJointInterface` Used to send torque commands for friction wheels and trigger wheel. 23 | 24 | ## Installation 25 | 26 | ### Installation from Packages 27 | 28 | To install all packages from this repository as Debian packages use 29 | 30 | ``` 31 | sudo apt-get install ros-noetic-rm-shooter-controllers 32 | ``` 33 | 34 | or better use `rosdep`: 35 | 36 | ``` 37 | sudo rosdep install --from-paths src 38 | ``` 39 | 40 | ### Dependencies 41 | 42 | - roscpp 43 | - rm_common 44 | - effort_controllers 45 | - dynamic_reconfigure 46 | 47 | ## Cfg 48 | 49 | + **shooter.cfg:** Add parameters related to friction wheel's angular velocity corresponding to each bullet speed and trigger block detection parameters. 50 | 51 | ## ROS API 52 | 53 | #### Subscribed Topics 54 | 55 | * **`command`** (rm_msgs/ShootCmd) 56 | 57 | Commands of controller state, bullet speed, frequency of shooting. 58 | 59 | #### Parameters 60 | 61 | * **`block_effort`, `block_speed`, `block_duration`** (double) 62 | 63 | When the torque of the trigger motor is greater than `block_effort` (in N·m), and the angular velocity of trigger motor is less than `block_speed` (in rad/s), it will be regarded as blocking if it continues for `block_duration` (in s), and the the state of shooter controller will switch to BLOCK. 64 | 65 | * **`block_overtime`** (double) 66 | 67 | If the time to enter BLOCK state exceeds `block_overtime` (in s), the state of shooter controller will switch to PUSH. 68 | 69 | * **`anti_block_angle`** (double) 70 | 71 | If shooter controller enter BLOCK state, the friction wheel will reverse `anti_block_angle` (in rad) to try to get rid of blocking. When the friction wheel get rid of BLOCK state successfully, the state of shooter controller will switch to PUSH. 72 | 73 | * **`anti_block_threshold`** (double) 74 | 75 | If the anti angle of the friction wheel exceeds `anti_block_threshold` (in rad), it means that friction wheel reverse successfully. 76 | 77 | * **`qd_10`, `qd_15`, `qd_18`, `qd_30`** (double) 78 | 79 | These parameters mean the friction wheel's angular velocity, the number of it's name expresses different bullet speeds (in m/s). 80 | 81 | ### Controller configuration examples 82 | 83 | #### Complete description 84 | 85 | ``` 86 | shooter_controller: 87 | type: rm_shooter_controllers/Controller 88 | publish_rate: 50 89 | friction_left: 90 | joint: "left_friction_wheel_joint" 91 | pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } 92 | friction_right: 93 | joint: "right_friction_wheel_joint" 94 | pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } 95 | trigger: 96 | joint: "trigger_joint" 97 | pid: { p: 50.0, i: 0.0, d: 1.5, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } 98 | push_per_rotation: 8 99 | push_qd_threshold: 0.90 100 | block_effort: 0.95 101 | block_duration: 0.05 102 | block_overtime: 0.5 103 | anti_block_angle: 0.2 104 | anti_block_threshold: 0.1 105 | qd_15: 460.0 106 | qd_18: 515.0 107 | qd_30: 740.0 108 | ``` 109 | 110 | ## Bugs & Feature Requests 111 | 112 | Please report bugs and request features using the [Issue Tracker](https://github.com/gdut-dynamic-x/simple_chassis_controller/issues) . 113 | -------------------------------------------------------------------------------- /rm_shooter_controllers/cfg/Shooter.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rm_shooter_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("block_effort", double_t, 0, "Trigger block effort", 0.95, 0.0, 10) 9 | gen.add("block_duration", double_t, 0, "Trigger block duration", 0.05, 0.0, 2.0) 10 | gen.add("block_speed", double_t, 0, "Trigger block speed", 0.5, 0.0, 5.) 11 | gen.add("block_overtime", double_t, 0, "Trigger block overtime", 0.5, 0.0, 5.) 12 | gen.add("anti_block_angle", double_t, 0, "Trigger anti block angle", 0.35, 0.0, 1.5) 13 | gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold", 0.05, 0.0, 0.2) 14 | gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) 15 | gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) 16 | gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) 17 | gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) 18 | gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) 19 | 20 | exit(gen.generate(PACKAGE, "shooter", "Shooter")) 21 | -------------------------------------------------------------------------------- /rm_shooter_controllers/doc/example.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rm-controls/rm_controllers/0bd8abd31d8f1b59765bae0da0429e2da4b8c71f/rm_shooter_controllers/doc/example.jpg -------------------------------------------------------------------------------- /rm_shooter_controllers/include/rm_shooter_controllers/standard.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by huakang on 2021/1/18. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | 56 | namespace rm_shooter_controllers 57 | { 58 | struct Config 59 | { 60 | double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, 61 | forward_push_threshold, exit_push_threshold; 62 | double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; 63 | }; 64 | 65 | class Controller : public controller_interface::MultiInterfaceController 67 | { 68 | public: 69 | Controller() = default; 70 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 71 | void update(const ros::Time& time, const ros::Duration& period) override; 72 | void starting(const ros::Time& /*time*/) override; 73 | 74 | private: 75 | void stop(const ros::Time& time, const ros::Duration& period); 76 | void ready(const ros::Duration& period); 77 | void push(const ros::Time& time, const ros::Duration& period); 78 | void block(const ros::Time& time, const ros::Duration& period); 79 | void setSpeed(const rm_msgs::ShootCmd& cmd); 80 | void normalize(); 81 | void judgeBulletShoot(const ros::Time& time, const ros::Duration& period); 82 | void commandCB(const rm_msgs::ShootCmdConstPtr& msg) 83 | { 84 | cmd_rt_buffer_.writeFromNonRT(*msg); 85 | } 86 | 87 | void reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/); 88 | 89 | hardware_interface::EffortJointInterface* effort_joint_interface_{}; 90 | std::vector> ctrls_friction_; 91 | effort_controllers::JointPositionController ctrl_trigger_; 92 | std::vector> wheel_speed_offsets_; 93 | std::vector> wheel_speed_directions_; 94 | LowPassFilter* lp_filter_; 95 | int push_per_rotation_{}, count_{}; 96 | double push_wheel_speed_threshold_{}; 97 | double freq_threshold_{}; 98 | bool dynamic_reconfig_initialized_ = false; 99 | bool state_changed_ = false; 100 | bool enter_ready_ = false; 101 | bool maybe_block_ = false; 102 | int friction_block_count = 0; 103 | bool last_friction_wheel_block = false, friction_wheel_block = false; 104 | double friction_block_effort_{}, friction_block_vel_{}; 105 | double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{}; 106 | bool has_shoot_ = false, has_shoot_last_ = false; 107 | bool wheel_speed_drop_ = true; 108 | double last_fricition_speed_{}; 109 | double last_friction_change_speed_{}; 110 | 111 | ros::Time last_shoot_time_, block_time_, last_block_time_; 112 | enum 113 | { 114 | STOP, 115 | READY, 116 | PUSH, 117 | BLOCK 118 | }; 119 | int state_ = STOP; 120 | Config config_{}; 121 | realtime_tools::RealtimeBuffer config_rt_buffer; 122 | realtime_tools::RealtimeBuffer cmd_rt_buffer_; 123 | rm_msgs::ShootCmd cmd_; 124 | std::shared_ptr> local_heat_state_pub_; 125 | std::shared_ptr> shoot_state_pub_; 126 | ros::Subscriber cmd_subscriber_; 127 | dynamic_reconfigure::Server* d_srv_{}; 128 | }; 129 | 130 | } // namespace rm_shooter_controllers 131 | -------------------------------------------------------------------------------- /rm_shooter_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rm_shooter_controllers 4 | 0.1.11 5 | RoboMaster standard robot Shooter controller 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | 15 | rm_common 16 | 17 | controller_interface 18 | effort_controllers 19 | dynamic_reconfigure 20 | 21 | 22 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rm_shooter_controllers/rm_shooter_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The Controller is RoboMaster standard robot 8 | Shooter controller. It expects a EffortJointInterface 9 | type of hardware interface. 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /rm_shooter_controllers/test/load_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /rm_shooter_controllers/test/shooter_config_template.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | shooter_controller: 3 | type: rm_shooter_controllers/Controller 4 | publish_rate: 50 5 | friction_left: 6 | joint: "left_friction_wheel_joint" 7 | pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } 8 | friction_right: 9 | joint: "right_friction_wheel_joint" 10 | pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } 11 | trigger: 12 | joint: "trigger_joint" 13 | pid: { p: 50.0, i: 0.0, d: 1.5, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } 14 | push_per_rotation: 8 15 | push_wheel_speed_threshold: 0.90 16 | block_effort: 0.95 17 | block_speed: 0.1 18 | block_duration: 0.05 19 | block_overtime: 0.5 20 | anti_block_angle: 0.2 21 | anti_block_threshold: 0.1 22 | -------------------------------------------------------------------------------- /robot_state_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package robot_state_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#86 `_ from NaHCO3bc/Readme 29 | Fix the dependence part bug. 30 | * Fix the dependence part bug. 31 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 32 | 0.1.7 33 | * Contributors: NaHCO3bc, ye-luo-xi-tui 34 | 35 | 0.1.7 (2022-09-10) 36 | ------------------ 37 | * Merge remote-tracking branch 'origin/master' 38 | * Contributors: qiayuan 39 | 40 | 0.1.6 (2022-06-16) 41 | ------------------ 42 | * Merge remote-tracking branch 'origin/master' 43 | * Contributors: qiayuan 44 | 45 | 0.1.5 (2022-06-10) 46 | ------------------ 47 | * Merge remote-tracking branch 'origin/master' 48 | * 0.1.4 49 | * Merge branch 'master' into gimbal_track 50 | * Merge pull request `#46 `_ from ye-luo-xi-tui/master 51 | Deprecated imu_filter_controller 52 | * Contributors: QiayuanLiao, YuuinIH, qiayuan, yezi 53 | 54 | 0.1.3 (2022-03-28) 55 | ------------------ 56 | * Merge branch 'master' into forward_feed 57 | * Merge branch 'master' into standard3 58 | * Merge remote-tracking branch 'origin/master' 59 | * Contributors: qiayuan, ye-luo-xi-tui, yezi 60 | 61 | 0.1.2 (2022-01-08) 62 | ------------------ 63 | * Merge branch 'master' of https://github.com/YuuinIH/rm_controllers 64 | * Merge branch 'master' into chassis/balance_imu_interface 65 | * Clear tf buffer when simulation reset 66 | * Merge branch 'gimbal/opti_or_simplify' into chassis/balance_imu_interface 67 | * Add the config of robot_state_controller, load only one controller on launch instead of spawn controllers 68 | * Merge branch 'master' into chassis/fix_filter 69 | * Merge remote-tracking branch 'origin/master' 70 | * Fix tf_transform cleared before publish 71 | * Check the timestamp when receiving the tf transforms outside the rm_hw/rm_gazebo 72 | * Real time safe for robot_state_controller 73 | * Merge branch 'namespace' 74 | # Conflicts: 75 | # rm_chassis_controllers/README.md 76 | * Merge pull request `#15 `_ from ye-luo-xi-tui/namespace 77 | Change name of namespace:from hardware_interface to rm_control 78 | * Change name of namespace:from hardware_interface to rm_control. 79 | * Remove README.md 80 | * Use “pragma once” in robot_state_controller headers instead of include guards. 81 | * Fix "robot_state_controller: CMakeLists.txt: error: unconfigured build_depend on 'pluginlib'" 82 | * Format robot_state_controller using clang-format 83 | * Contributors: BruceLannn, MuZhou233, QiayuanLiao, YuuinIH, chenzheng, qiayuan, yezi 84 | 85 | 0.1.1 (2021-08-12) 86 | ------------------ 87 | * Set all version to the same 88 | * Add license to robot_state_controller source files 89 | * Merge remote-tracking branch 'alias_memory/metapackage' 90 | * Move all files to robot_state_controller/robot_state_controller, prepare for merge 91 | * Contributors: qiayuan 92 | -------------------------------------------------------------------------------- /robot_state_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(robot_state_controller) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | roscpp 16 | 17 | rm_common 18 | controller_interface 19 | tf2_kdl 20 | kdl_parser 21 | realtime_tools 22 | ) 23 | 24 | ################################### 25 | ## catkin specific configuration ## 26 | ################################### 27 | ## The catkin_package macro generates cmake config files for your package 28 | ## Declare things to be passed to dependent projects 29 | ## INCLUDE_DIRS: uncomment this if your package contains header files 30 | ## LIBRARIES: libraries you create in this project that dependent projects also need 31 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 32 | ## DEPENDS: system dependencies of this project that dependent projects also need 33 | catkin_package( 34 | INCLUDE_DIRS 35 | include 36 | LIBRARIES 37 | ${PROJECT_NAME} 38 | CATKIN_DEPENDS 39 | roscpp 40 | 41 | rm_common 42 | controller_interface 43 | kdl_parser 44 | realtime_tools 45 | LIBRARIES ${PROJECT_NAME} 46 | ) 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | ## Specify additional locations of header files 53 | ## Your package locations should be listed before other locations 54 | include_directories( 55 | include 56 | ${catkin_INCLUDE_DIRS} 57 | ) 58 | 59 | ## Declare cpp executables 60 | add_library(${PROJECT_NAME} 61 | src/${PROJECT_NAME}.cpp 62 | ) 63 | 64 | target_link_libraries(${PROJECT_NAME} 65 | ${catkin_LIBRARIES} 66 | ) 67 | 68 | ############# 69 | ## Install ## 70 | ############# 71 | 72 | ## Mark executables and/or libraries for installation 73 | install( 74 | TARGETS ${PROJECT_NAME} 75 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 78 | ) 79 | 80 | # Mark cpp header files for installation 81 | install( 82 | DIRECTORY include/${PROJECT_NAME}/ 83 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 84 | FILES_MATCHING PATTERN "*.h" 85 | ) 86 | install( 87 | FILES robot_state_controller_plugin.xml 88 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 89 | ) 90 | 91 | ## Mark other files for installation 92 | install( 93 | DIRECTORY test 94 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 95 | ) 96 | 97 | ############# 98 | ## Testing ## 99 | ############# 100 | 101 | #if (${CATKIN_ENABLE_TESTING}) 102 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 103 | # ## Add gtest based cpp test target and link libraries 104 | # catkin_add_gtest(${PROJECT_NAME}-test 105 | # test/test_ros_package_template.cpp 106 | # test/AlgorithmTest.cpp) 107 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) 108 | #endif () 109 | -------------------------------------------------------------------------------- /robot_state_controller/README.md: -------------------------------------------------------------------------------- 1 | # robot_state_controller 2 | 3 | ## Overview 4 | 5 | The robot_state_controller uses the URDF specified by the parameter robot_description and the joint positions from the `hardware_interface::JointStateInterface` to calculate the forward kinematics of the robot and publish the results via tf, and reads and manages the external incoming tf. 6 | 7 | **keywords:** ROS, urdf, transformation, joint 8 | 9 | #### License 10 | 11 | The source code is released under a [BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). 12 | 13 | **Author: DynamicX
14 | Affiliation: DynamicX
15 | Maintainer: DynamicX** 16 | 17 | The robot_state_controller package has been tested under [ROS](http://www.ros.org) Melodic and Noetic on respectively 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 18 | 19 | ### Hardware interface type 20 | 21 | + `JointStateInterface` Used to get the positions of different joints. 22 | + `RobotStateInterface` Used to obtain and maintain the transformation relationship of each link of the entire robot. 23 | 24 | ## Installation 25 | 26 | ### Installation from Packages 27 | 28 | To install all packages from this repository as Debian packages use 29 | 30 | ```shell 31 | sudo apt-get install ros-noetic-robot-state-controller 32 | ``` 33 | 34 | or better use `rosdep`: 35 | 36 | ```shell 37 | sudo rosdep install --from-paths src 38 | ``` 39 | 40 | ### Dependencies 41 | 42 | * roscpp 43 | * rm_common 44 | * controller_interface 45 | * tf2_kdl 46 | * kdl_parser 47 | * realtime_tools 48 | 49 | ## ROS API 50 | 51 | #### Subscribed Topics 52 | 53 | * **`tf`** (tf2_msgs/TFMessage) 54 | 55 | Obtain and manage maintenance real-time transformation information. 56 | 57 | * **`tf_static`**(tf2_msgs/TFMessage) 58 | 59 | Obtain and manage maintenance static transformation information. 60 | 61 | #### Parameters 62 | 63 | * **`publish_rate`** (double) 64 | 65 | Publish frequency of state publisher, default: 50Hz. 66 | 67 | * **`use_tf_static`** (bool) 68 | 69 | Set whether to use the /tf_static latched static transform broadcaster.Default: true. 70 | 71 | * **`ignore_timestamp`** (bool) 72 | 73 | If true, ignore the publish_frequency and the timestamp of joint_states and publish a tf for each of the received joint_states. Default: false. 74 | 75 | * **`buffer_duration`** (double) 76 | 77 | The time to keep a history of transforms. 78 | 79 | * **`robot_description`** (string) 80 | 81 | The urdf xml robot description. 82 | 83 | #### Complete description 84 | 85 | ```yaml 86 | robot_state_controller: 87 | type: robot_state_controller/RobotStateController 88 | publish_rate: 100 89 | ``` 90 | 91 | ## Bugs & Feature Requests 92 | 93 | Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). 94 | -------------------------------------------------------------------------------- /robot_state_controller/include/robot_state_controller/robot_state_controller.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, Qiayuan Liao 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *******************************************************************************/ 33 | 34 | // 35 | // Created by qiayuan on 1/3/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace robot_state_controller 54 | { 55 | class SegmentPair 56 | { 57 | public: 58 | SegmentPair(const KDL::Segment& p_segment, std::string p_root, std::string p_tip) 59 | : segment(p_segment), root(std::move(p_root)), tip(std::move(p_tip)) 60 | { 61 | } 62 | 63 | KDL::Segment segment{}; 64 | std::string root, tip; 65 | }; 66 | 67 | class RobotStateController 68 | : public controller_interface::MultiInterfaceController 70 | { 71 | public: 72 | RobotStateController() = default; 73 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 74 | void update(const ros::Time& time, const ros::Duration& /*period*/) override; 75 | 76 | private: 77 | void addChildren(KDL::SegmentMap::const_iterator segment); 78 | void tfSubCallback(const tf2_msgs::TFMessageConstPtr& msg); 79 | void staticSubCallback(const tf2_msgs::TFMessageConstPtr& msg); 80 | 81 | urdf::Model model_{}; 82 | std::map* mimic_{}; 83 | unsigned int num_hw_joints_{}; 84 | bool use_tf_static_{}; 85 | bool ignore_timestamp_{}; 86 | double publish_rate_{}; 87 | ros::Time last_update_; 88 | ros::Time last_publish_time_; 89 | 90 | std::map jnt_states_; 91 | std::map segments_, segments_fixed_; 92 | 93 | tf2_ros::Buffer* tf_buffer_{}; 94 | rm_common::TfRtBroadcaster tf_broadcaster_; 95 | rm_common::StaticTfRtBroadcaster static_tf_broadcaster_; 96 | // Do not use tf2_ros::TransformListener because it will lead to setTransform calling twice when publishing the transform 97 | ros::Subscriber tf_sub_; 98 | ros::Subscriber tf_static_sub_; 99 | realtime_tools::RealtimeBuffer tf_msg_; 100 | realtime_tools::RealtimeBuffer tf_static_msg_; 101 | }; 102 | 103 | } // namespace robot_state_controller 104 | -------------------------------------------------------------------------------- /robot_state_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_state_controller 4 | 0.1.11 5 | A template for ROS packages. 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | 15 | rm_common 16 | controller_interface 17 | kdl_parser 18 | tf2_kdl 19 | realtime_tools 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /robot_state_controller/robot_state_controller_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | This controller publishes the readings of all available tf by read the joint_data 8 | The controller expects a JointStateInterface type of hardware interface. 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /robot_state_controller/test/config.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | robot_state_controller: 3 | type: robot_state_controller/RobotStateController 4 | publish_rate: 100 5 | joint_state_controller: 6 | type: joint_state_controller/JointStateController 7 | publish_rate: 100 8 | -------------------------------------------------------------------------------- /robot_state_controller/test/load_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /tof_radar_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package tof_radar_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.11 (2023-06-20) 6 | ------------------- 7 | * Merge branch 'master' into dev/balance 8 | * Merge pull request `#120 `_ from ye-luo-xi-tui/master 9 | 0.1.10 10 | * Merge branch 'rm-controls:master' into gpio_calibration_controller 11 | * Contributors: 1moule, ye-luo-xi-tui, yuchen 12 | 13 | 0.1.10 (2023-03-25) 14 | ------------------- 15 | * Merge pull request `#106 `_ from ye-luo-xi-tui/master 16 | 0.1.9 17 | * Contributors: ye-luo-xi-tui 18 | 19 | 0.1.9 (2023-02-21) 20 | ------------------ 21 | * Merge branch 'master' into balance_standard 22 | * Merge pull request `#97 `_ from ye-luo-xi-tui/master 23 | 0.1.8 24 | * Contributors: ye-luo-xi-tui, yezi 25 | 26 | 0.1.8 (2022-11-24) 27 | ------------------ 28 | * Merge pull request `#86 `_ from NaHCO3bc/Readme 29 | Fix the dependence part bug. 30 | * Modify the test file folder. 31 | * Fix the dependence part bug. 32 | * Merge pull request `#85 `_ from ye-luo-xi-tui/master 33 | 0.1.7 34 | * Contributors: NaHCO3bc, ye-luo-xi-tui 35 | 36 | 0.1.7 (2022-09-10) 37 | ------------------ 38 | * Merge remote-tracking branch 'origin/master' 39 | * Contributors: qiayuan 40 | 41 | 0.1.6 (2022-06-16) 42 | ------------------ 43 | * Merge pull request `#76 `_ from Edwinlinks/tof-radar-controller 44 | Finished tof radar controller and delete tof sensor controller 45 | * Change exec depend tof_sensor_controller to tof_radar_controller 46 | * Modify format error 47 | * Modify tof sensor controller to tof radar controller 48 | * Contributors: Edwinlinks, QiayuanLiao 49 | -------------------------------------------------------------------------------- /tof_radar_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tof_radar_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | 7 | rm_common 8 | 9 | 10 | pluginlib 11 | controller_interface 12 | realtime_tools 13 | ) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS 17 | include 18 | CATKIN_DEPENDS 19 | roscpp 20 | 21 | rm_common 22 | 23 | pluginlib 24 | controller_interface 25 | realtime_tools 26 | LIBRARIES ${PROJECT_NAME} 27 | ) 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | add_library(${PROJECT_NAME} 35 | src/tof_radar_controller.cpp 36 | ) 37 | 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | install( 43 | TARGETS ${PROJECT_NAME} 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 47 | ) 48 | 49 | # Mark cpp header files for installation 50 | install( 51 | DIRECTORY include/${PROJECT_NAME}/ 52 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 53 | FILES_MATCHING PATTERN "*.h" 54 | ) 55 | 56 | # Mark other files for installation 57 | install( 58 | DIRECTORY test 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 60 | ) 61 | install( 62 | FILES tof_radar_controller_plugins.xml 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 64 | ) 65 | -------------------------------------------------------------------------------- /tof_radar_controller/include/tof_radar_controller/tof_radar_controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by luotinkai on 2022/1/2. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace tof_radar_controller 14 | { 15 | class Controller : public controller_interface::Controller 16 | { 17 | public: 18 | Controller() = default; 19 | 20 | bool init(rm_control::TofRadarInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; 21 | 22 | void update(const ros::Time& time, const ros::Duration& period) override; 23 | 24 | private: 25 | std::vector tof_radar_; 26 | typedef std::shared_ptr> RtpublisherPtr; 27 | RtpublisherPtr tof_pub_; 28 | std::vector realtime_pubs_; 29 | }; 30 | } // namespace tof_radar_controller 31 | -------------------------------------------------------------------------------- /tof_radar_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tof_radar_controller 4 | 0.1.11 5 | The tof radar controller package 6 | luotinkai 7 | 8 | BSD 9 | 10 | catkin 11 | 12 | rm_common 13 | pluginlib 14 | roscpp 15 | controller_interface 16 | realtime_tools 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /tof_radar_controller/src/tof_radar_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by luotinkai on 2022/1/2. 3 | // 4 | 5 | #include "tof_radar_controller/tof_radar_controller.h" 6 | #include 7 | 8 | namespace tof_radar_controller 9 | { 10 | bool Controller::init(rm_control::TofRadarInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) 11 | { 12 | const std::vector& tof_radar_names = hw->getNames(); 13 | for (const auto& tof_radar_name : tof_radar_names) 14 | ROS_DEBUG("Got radar %s", tof_radar_name.c_str()); 15 | 16 | for (const auto& tof_radar_name : tof_radar_names) 17 | { 18 | // sensor handle 19 | tof_radar_.push_back(hw->getHandle(tof_radar_name)); 20 | 21 | // realtime publisher 22 | tof_pub_.reset( 23 | new realtime_tools::RealtimePublisher(controller_nh, tof_radar_name + "/data", 100)); 24 | realtime_pubs_.push_back(tof_pub_); 25 | } 26 | return true; 27 | } 28 | 29 | void Controller::update(const ros::Time& time, const ros::Duration& period) 30 | { 31 | for (unsigned int i = 0; i < realtime_pubs_.size(); ++i) 32 | { 33 | if (realtime_pubs_[i]->trylock()) 34 | { 35 | realtime_pubs_[i]->msg_.distance = tof_radar_[i].getDistance() / 100.; 36 | realtime_pubs_[i]->msg_.strength = tof_radar_[i].getStrength(); 37 | realtime_pubs_[i]->msg_.stamp = time; 38 | realtime_pubs_[i]->unlockAndPublish(); 39 | } 40 | } 41 | } 42 | 43 | } // namespace tof_radar_controller 44 | 45 | PLUGINLIB_EXPORT_CLASS(tof_radar_controller::Controller, controller_interface::ControllerBase) 46 | -------------------------------------------------------------------------------- /tof_radar_controller/test/load_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | -------------------------------------------------------------------------------- /tof_radar_controller/test/tof_config_template.yaml: -------------------------------------------------------------------------------- 1 | tof_radar_controller: 2 | type: tof_radar_controller/Controller 3 | publish_rate: 50 4 | -------------------------------------------------------------------------------- /tof_radar_controller/tof_radar_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | --------------------------------------------------------------------------------