├── .clang-format ├── .clang-tidy ├── .github └── workflows │ └── format.yml ├── .pre-commit-config.yaml ├── README.md ├── rc_common ├── CMakeLists.txt ├── include │ └── rc_common │ │ ├── eigen_types.h │ │ ├── filters │ │ ├── filters.h │ │ ├── imu_complementary_filter.h │ │ ├── imu_filter_base.h │ │ ├── kalman_filter.h │ │ ├── lp_filter.h │ │ └── one_euro_filter.h │ │ ├── hardware_interface │ │ ├── action_interface.h │ │ ├── actuator_extra_interface.h │ │ ├── dt35_interface.h │ │ ├── gpio_interface.h │ │ ├── rc_imu_sensor_interface.h │ │ └── robot_state_interface.h │ │ ├── linear_interpolation.h │ │ ├── lqr.h │ │ ├── math_utilities.h │ │ ├── ori_tool.h │ │ ├── ros_utilities.h │ │ ├── tf_rt_broadcaster.h │ │ └── traj_gen.h ├── package.xml └── src │ ├── filter │ ├── filters.cpp │ ├── imu_complementary_filter.cpp │ ├── imu_filter_base.cpp │ └── lp_filter.cpp │ ├── ori_tool.cpp │ └── tf_rt_broadcaster.cpp ├── rc_gazebo ├── CMakeLists.txt ├── config │ └── actions.yaml ├── include │ └── rc_gazebo │ │ └── rc_action_hw_sim.h ├── launch │ ├── empty_world.launch │ ├── field_rc21.launch │ ├── field_rc23.launch │ └── task_model.launch ├── models │ └── task_model │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── rc_action_hw_sim_plugins.xml ├── src │ └── rc_action_hw_sim.cpp └── worlds │ ├── empty.world │ └── field_rc23.world ├── rc_hw ├── CMakeLists.txt ├── config │ ├── actuator_coefficient.yaml │ └── hw_config_template.yaml ├── include │ └── rc_hw │ │ ├── control_loop.h │ │ └── hardware_interface │ │ ├── DT35_laser.h │ │ ├── action_manager.h │ │ ├── can_bus.h │ │ ├── gpio_manager.h │ │ ├── hardware_interface.h │ │ ├── socketcant.h │ │ └── types.h ├── launch │ └── rc_hw.launch ├── package.xml ├── rc_hw_loader_plugins.xml ├── src │ ├── control_loop.cpp │ ├── hardware_interface │ │ ├── DT35_laser.cpp │ │ ├── action_manager.cpp │ │ ├── can_bus.cpp │ │ ├── gpio_manager.cpp │ │ ├── hardware_interface.cpp │ │ ├── parse.cpp │ │ └── socketcan.cpp │ └── rc_hw.cpp └── test │ ├── test_hw.launch │ └── test_hw.yaml ├── rc_ibus ├── CMakeLists.txt ├── config │ └── rc_ibus.yaml ├── doc │ └── fs_i6s.png ├── include │ └── rc_ibus │ │ ├── ibus.h │ │ └── ibus_node.h ├── launch │ └── rc_ibus.launch ├── package.xml └── src │ ├── ibus.cpp │ └── ibus_node.cpp └── rc_msgs ├── CMakeLists.txt ├── msg ├── ActionData.msg ├── ActuatorState.msg ├── ChassisCmd.msg ├── GimbalCmd.msg ├── GimbalDesError.msg ├── GpioData.msg ├── IbusData.msg ├── LpData.msg └── ShooterCmd.msg ├── package.xml └── srv └── ActionCmd.srv /.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/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-20.04 15 | steps: 16 | - uses: actions/checkout@v2 17 | - uses: actions/setup-python@v2 18 | - name: Install clang-format-10 19 | run: | 20 | wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - 21 | sudo apt-get install clang-format-10 22 | - name: Install catkin-lint 23 | run: | 24 | lsb_release -sc 25 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 26 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 27 | sudo apt-get -q update 28 | sudo apt-get -q install python3-rosdep 29 | sudo rm -rf /etc/ros/rosdep/sources.list.d/* 30 | sudo rosdep init 31 | rosdep update 32 | sudo apt-get -q install catkin-lint 33 | export ROS_DISTRO=noetic 34 | - uses: pre-commit/action@v2.0.0 35 | -------------------------------------------------------------------------------- /.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 | #first test 15 | 16 | repos: 17 | # Standard hooks 18 | - repo: https://github.com/pre-commit/pre-commit-hooks 19 | rev: v4.0.1 20 | hooks: 21 | - id: check-added-large-files 22 | args: [ '--maxkb=2000' ] 23 | - id: check-case-conflict 24 | - id: check-merge-conflict 25 | - id: check-symlinks 26 | - id: check-yaml 27 | - id: debug-statements 28 | - id: end-of-file-fixer 29 | - id: mixed-line-ending 30 | - id: trailing-whitespace 31 | 32 | - repo: https://github.com/psf/black 33 | rev: 20.8b1 34 | hooks: 35 | - id: black 36 | 37 | - repo: https://github.com/Lucas-C/pre-commit-hooks-markup 38 | rev: v1.0.1 39 | hooks: 40 | - id: rst-linter 41 | exclude: .*/doc/.* 42 | 43 | - repo: local 44 | hooks: 45 | - id: clang-format 46 | name: clang-format 47 | description: Format files with ClangFormat. 48 | entry: clang-format-10 49 | language: system 50 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 51 | args: [ "-fallback-style=none", "-i" ] 52 | - id: catkin_lint 53 | name: catkin_lint 54 | description: Check package.xml and cmake files 55 | entry: catkin_lint . 56 | language: system 57 | always_run: true 58 | pass_filenames: false 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rc_control 2 | -------------------------------------------------------------------------------- /rc_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rc_common) 3 | 4 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 5 | ## enforcing cleaner code. 6 | add_definitions(-Wall -Werror -Wno-enum-compare) 7 | 8 | find_package(Eigen3 REQUIRED) 9 | 10 | find_package(catkin REQUIRED 11 | COMPONENTS 12 | roscpp 13 | roslint 14 | tf 15 | rc_msgs 16 | geometry_msgs 17 | control_msgs 18 | controller_manager_msgs 19 | imu_complementary_filter 20 | imu_filter_madgwick 21 | realtime_tools 22 | dynamic_reconfigure 23 | ) 24 | 25 | catkin_package( 26 | INCLUDE_DIRS 27 | include 28 | ${EIGEN3_INCLUDE_DIR} 29 | CATKIN_DEPENDS 30 | tf 31 | rc_msgs 32 | geometry_msgs 33 | control_msgs 34 | controller_manager_msgs 35 | imu_complementary_filter 36 | imu_filter_madgwick 37 | roscpp 38 | dynamic_reconfigure 39 | DEPENDS 40 | LIBRARIES 41 | rc_common 42 | ) 43 | 44 | include_directories( 45 | include 46 | ${catkin_INCLUDE_DIRS} 47 | ${EIGEN3_INCLUDE_DIR} 48 | ) 49 | 50 | file(GLOB_RECURSE sources "src/*.cpp" "src/filter/*.cpp") 51 | 52 | add_library(rc_common SHARED ${sources}) 53 | #add_executable(test_traj test/test_traj.cpp) 54 | #add_executable(test_kalman test/test_kalman_filter.cpp) 55 | 56 | target_link_libraries(rc_common ${catkin_LIBRARIES}) 57 | #target_link_libraries(test_traj rc_common ${catkin_LIBRARIES}) 58 | #target_link_libraries(test_kalman rc_common ${catkin_LIBRARIES}) 59 | 60 | # Fix rc_msgs generation problem 61 | # See https://answers.ros.org/question/73048 62 | add_dependencies(rc_common rc_msgs_generate_messages_cpp) 63 | 64 | ############# 65 | ## Install ## 66 | ############# 67 | 68 | # Mark executables and/or libraries for installation 69 | install( 70 | TARGETS ${PROJECT_NAME} 71 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 74 | ) 75 | 76 | # Mark cpp header files for installation 77 | install( 78 | DIRECTORY include/${PROJECT_NAME}/ 79 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 80 | FILES_MATCHING PATTERN "*.h" 81 | ) 82 | 83 | # Mark other files for installation 84 | #install( 85 | # DIRECTORY doc 86 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | #) 88 | 89 | ############# 90 | ## Testing ## 91 | ############# 92 | 93 | ## Add gtest based cpp test target and link libraries 94 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rc_common.cpp) 95 | # if(TARGET ${PROJECT_NAME}-test) 96 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 97 | # endif() 98 | 99 | ## Add folders to be run by python nosetests 100 | # catkin_add_nosetests(test) 101 | roslint_cpp() 102 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/eigen_types.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/3/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | 42 | #include 43 | 44 | // Rotation Matrix 45 | template 46 | using RotMat = typename Eigen::Matrix; 47 | 48 | // 2x1 Vector 49 | template 50 | using Vec2 = typename Eigen::Matrix; 51 | 52 | // 3x1 Vector 53 | template 54 | using Vec3 = typename Eigen::Matrix; 55 | 56 | // 4x1 Vector 57 | template 58 | using Vec4 = typename Eigen::Matrix; 59 | 60 | // 6x1 Vector 61 | template 62 | using Vec6 = Eigen::Matrix; 63 | 64 | // 8x1 Vector 65 | template 66 | using Vec8 = Eigen::Matrix; 67 | 68 | // 10x1 Vector 69 | template 70 | using Vec10 = Eigen::Matrix; 71 | 72 | // 12x1 Vector 73 | template 74 | using Vec12 = Eigen::Matrix; 75 | 76 | // 18x1 Vector 77 | template 78 | using Vec18 = Eigen::Matrix; 79 | 80 | // 28x1 vector 81 | template 82 | using Vec28 = Eigen::Matrix; 83 | 84 | // 2x2 Matrix 85 | template 86 | using Mat2 = typename Eigen::Matrix; 87 | 88 | // 3x3 Matrix 89 | template 90 | using Mat3 = typename Eigen::Matrix; 91 | 92 | // 8x8 Matrix 93 | template 94 | using Mat8 = typename Eigen::Matrix; 95 | 96 | // 4x1 Vector 97 | template 98 | using Quat = typename Eigen::Matrix; 99 | 100 | // Spatial Vector (6x1, all subspaces) 101 | template 102 | using SVec = typename Eigen::Matrix; 103 | 104 | // Spatial Transform (6x6) 105 | template 106 | using SXform = typename Eigen::Matrix; 107 | 108 | // 6x6 Matrix 109 | template 110 | using Mat6 = typename Eigen::Matrix; 111 | 112 | // 12x12 Matrix 113 | template 114 | using Mat12 = typename Eigen::Matrix; 115 | 116 | // 18x18 Matrix 117 | template 118 | using Mat18 = Eigen::Matrix; 119 | 120 | // 28x28 Matrix 121 | template 122 | using Mat28 = Eigen::Matrix; 123 | 124 | // 3x4 Matrix 125 | template 126 | using Mat34 = Eigen::Matrix; 127 | 128 | // 3x4 Matrix 129 | template 130 | using Mat23 = Eigen::Matrix; 131 | 132 | // 4x4 Matrix 133 | template 134 | using Mat4 = typename Eigen::Matrix; 135 | 136 | // 10x1 Vector 137 | template 138 | using MassProperties = typename Eigen::Matrix; 139 | 140 | // Dynamically sized vector 141 | template 142 | using DVec = typename Eigen::Matrix; 143 | 144 | // Dynamically sized matrix 145 | template 146 | using DMat = typename Eigen::Matrix; 147 | 148 | // Dynamically sized matrix with spatial vector columns 149 | template 150 | using D6Mat = typename Eigen::Matrix; 151 | 152 | // Dynamically sized matrix with cartesian vector columns 153 | template 154 | using D3Mat = typename Eigen::Matrix; 155 | 156 | // std::vector (a list) of Eigen things 157 | template 158 | using vectorAligned = typename std::vector>; 159 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/filters.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 | #pragma once 35 | 36 | template 37 | class Filter 38 | { 39 | public: 40 | Filter() = default; 41 | virtual ~Filter() = default; 42 | virtual void input(T input_value) = 0; 43 | virtual T output() = 0; 44 | virtual void clear() = 0; 45 | }; 46 | 47 | template 48 | class ButterworthFilter : public Filter 49 | { 50 | public: 51 | ButterworthFilter(int num_sample, T dt, T cutoff_frequency); 52 | ~ButterworthFilter(); 53 | void input(T input_value); 54 | T output(); 55 | void clear(); 56 | 57 | private: 58 | T* mpBuffer_; 59 | int mCurIdx_; 60 | int mNumSample_; 61 | T mDt_; 62 | T mCutoffFreq_; 63 | T mValue_; 64 | }; 65 | 66 | template 67 | class DigitalLpFilter : public Filter 68 | { 69 | public: 70 | DigitalLpFilter(T w_c, T t_s); 71 | ~DigitalLpFilter(); 72 | void input(T input_value); 73 | T output(); 74 | void clear(); 75 | 76 | private: 77 | T Lpf_in_prev_[2]; 78 | T Lpf_out_prev_[2]; 79 | T Lpf_in1_, Lpf_in2_, Lpf_in3_, Lpf_out1_, Lpf_out2_; 80 | T lpf_out_; 81 | }; 82 | 83 | template 84 | class MovingAverageFilter : public Filter 85 | { 86 | public: 87 | explicit MovingAverageFilter(int num_data); 88 | ~MovingAverageFilter(); 89 | void input(T input_value); 90 | T output(); 91 | void clear(); 92 | 93 | private: 94 | T* buffer_; 95 | int num_data_; 96 | int idx_; 97 | T sum_; 98 | }; 99 | 100 | template 101 | class DerivLpFilter : public Filter 102 | { 103 | public: 104 | DerivLpFilter(T w_c, T t_s); 105 | ~DerivLpFilter(); 106 | void input(T input_value); 107 | T output(); 108 | void clear(); 109 | 110 | private: 111 | T Lpf_in_prev_[2]; 112 | T Lpf_out_prev_[2]; 113 | T Lpf_in1_, Lpf_in2_, Lpf_in3_, Lpf_out1_, Lpf_out2_; 114 | T lpf_out_; 115 | }; 116 | 117 | template 118 | class FF01Filter : public Filter 119 | { 120 | public: 121 | FF01Filter(float t_s, float w_c); 122 | virtual ~FF01Filter(); 123 | virtual void input(T input_value); 124 | virtual T output(); 125 | virtual void clear(); 126 | 127 | private: 128 | T Lpf_in_prev_[2]; 129 | T Lpf_out_prev_[2]; 130 | T Lpf_in1_, Lpf_in2_, Lpf_in3_, Lpf_out1_, Lpf_out2_; 131 | T lpf_out_; 132 | }; 133 | 134 | template 135 | class FF02Filter : public Filter 136 | { 137 | public: 138 | FF02Filter(float t_s, float w_c); 139 | ~FF02Filter(); 140 | void input(T input_value); 141 | T output(); 142 | void clear(); 143 | 144 | private: 145 | T Lpf_in_prev_[2]; 146 | T Lpf_out_prev_[2]; 147 | T Lpf_in1_, Lpf_in2_, Lpf_in3_, Lpf_out1_, Lpf_out2_; 148 | T lpf_out_; 149 | }; 150 | 151 | template 152 | class AverageFilter : public Filter 153 | { 154 | public: 155 | AverageFilter(T dt, T t_const, T limit); 156 | ~AverageFilter(); 157 | void input(T input_value); 158 | T output(); 159 | void clear(); 160 | 161 | private: 162 | T est_value_; 163 | T dt_; 164 | T t_const_; 165 | T limit_; 166 | }; 167 | 168 | template 169 | class RampFilter : public Filter 170 | { 171 | public: 172 | RampFilter(T acc, T dt); 173 | ~RampFilter() = default; 174 | void input(T input_value); 175 | void clear(); 176 | void clear(T last_value); 177 | void setAcc(T acc); // without clear. 178 | T output(); 179 | 180 | private: 181 | T last_value_; 182 | T acc_; 183 | T dt_; 184 | }; 185 | 186 | template 187 | class OneEuroFilter : public Filter 188 | { 189 | public: 190 | OneEuroFilter(double _freq, T _mincutoff, T _beta, T _dcutoff); 191 | ~OneEuroFilter(); 192 | void input(T input_value); 193 | T output(); 194 | void clear(); 195 | 196 | private: 197 | double freq; 198 | bool firsttime; 199 | T mincutoff, beta, dcutoff; 200 | T x_prev, dhatxprev, hatxprev; 201 | T filtered_val; 202 | }; 203 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/imu_complementary_filter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2022/3/26. 3 | // 4 | 5 | #pragma once 6 | 7 | #include "rc_common/filters/imu_filter_base.h" 8 | #include 9 | 10 | namespace rc_common { 11 | class ImuComplementaryFilter : public ImuFilterBase { 12 | public: 13 | ImuComplementaryFilter() = default; 14 | void getOrientation(double &q0, double &q1, double &q2, double &q3) override; 15 | 16 | private: 17 | void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) override; 18 | bool initFilter(XmlRpc::XmlRpcValue &imu_data) override; 19 | void resetFilter() override; 20 | // Parameters: 21 | double gain_acc_; 22 | double gain_mag_; 23 | bool do_bias_estimation_; 24 | double bias_alpha_; 25 | bool do_adaptive_gain_; 26 | bool use_mag_; 27 | std::shared_ptr filter_; 28 | }; 29 | }// namespace rc_common 30 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/imu_filter_base.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2022/3/26. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace rc_common { 14 | class ImuFilterBase { 15 | public: 16 | bool init(XmlRpc::XmlRpcValue &imu_data, const std::string &name); 17 | void update(ros::Time time, double *accel, double *omega, double *ori, double *accel_cov, double *omega_cov, 18 | double *ori_cov, double temp, bool camera_trigger); 19 | virtual void getOrientation(double &q0, double &q1, double &q2, double &q3) = 0; 20 | 21 | protected: 22 | virtual bool initFilter(XmlRpc::XmlRpcValue &imu_data) = 0; 23 | virtual void resetFilter() = 0; 24 | virtual void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) = 0; 25 | ros::Time last_update_; 26 | bool initialized_filter_{false}; 27 | std::string frame_id_; 28 | ros::Publisher imu_data_pub_; 29 | ros::Publisher imu_temp_pub_; 30 | ros::Publisher trigger_time_pub_; 31 | sensor_msgs::Imu imu_pub_data_; 32 | sensor_msgs::Temperature temp_pub_data_; 33 | sensor_msgs::TimeReference trigger_time_pub_data_; 34 | }; 35 | }// namespace rc_common 36 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/kalman_filter.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/3/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include "rc_common/eigen_types.h" 41 | #include 42 | 43 | template 44 | class KalmanFilter { 45 | public: 46 | template 47 | KalmanFilter(const Eigen::MatrixBase &A, const Eigen::MatrixBase &B, const Eigen::MatrixBase &H, 48 | const Eigen::MatrixBase &Q, const Eigen::MatrixBase &R) 49 | : A_(A), B_(B), H_(H), Q_(Q), R_(R), m_(TH::RowsAtCompileTime), n_(TA::RowsAtCompileTime), inited(false) { 50 | // Check dimension 51 | // ref:http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/MatrixKalman.html 52 | static_assert(TA::RowsAtCompileTime == TA::ColsAtCompileTime, "A should be square matrix"); 53 | static_assert(TA::RowsAtCompileTime == TH::ColsAtCompileTime, "H columns should be equal to A columns"); 54 | static_assert(TB::RowsAtCompileTime == TA::ColsAtCompileTime, "B rows should be equal to A columns "); 55 | static_assert(TQ::RowsAtCompileTime == TA::ColsAtCompileTime && TQ::ColsAtCompileTime == TA::ColsAtCompileTime, 56 | "The rows and columns of Q should be equal to the columns of A"); 57 | static_assert(TR::RowsAtCompileTime == TH::RowsAtCompileTime && TR::ColsAtCompileTime == TH::RowsAtCompileTime, 58 | "The rows and columns of Q should be equal to the rows of H"); 59 | x_.resize(n_); 60 | P_.resize(n_, n_); 61 | I_.resize(n_, n_); 62 | I_.setIdentity(); 63 | } 64 | 65 | ~KalmanFilter() = default; 66 | 67 | template 68 | void clear(const Eigen::MatrixBase &x) { 69 | x_ = x; 70 | inited = true; 71 | K_ = DMat::Zero(n_, m_); 72 | P_ = DMat::Zero(n_, m_); 73 | P_new_ = DMat::Zero(n_, n_); 74 | } 75 | 76 | template 77 | void update(const Eigen::MatrixBase &z) { 78 | update(z, R_); 79 | }; 80 | 81 | template 82 | void update(const Eigen::MatrixBase &z, const Eigen::MatrixBase &R) { 83 | if (!inited) 84 | return;// TODO: add assert 85 | // update R_ 86 | R_ = R; 87 | // update 88 | K_ = P_new_ * H_.transpose() * ((H_ * P_new_ * H_.transpose() + R_).inverse()); 89 | x_ = x_ + K_ * (z - H_ * x_); 90 | P_ = (I_ - K_ * H_) * P_new_; 91 | }; 92 | 93 | template 94 | void predict(const Eigen::MatrixBase &u) { 95 | predict(u, Q_); 96 | } 97 | 98 | template 99 | void predict(const Eigen::MatrixBase &u, const Eigen::MatrixBase &Q) { 100 | if (!inited) 101 | return;// TODO: add assert 102 | // update Q_ 103 | Q_ = Q; 104 | // predict 105 | x_ = A_ * x_ + B_ * u; 106 | P_new_ = A_ * P_ * A_.transpose() + Q_; 107 | } 108 | 109 | DVec getState() { 110 | return x_; 111 | } 112 | 113 | private: 114 | DMat A_, B_, H_, I_; 115 | DMat Q_, R_, P_, P_new_, K_; 116 | DVec x_; 117 | const int m_, n_;// dimension 118 | bool inited; 119 | }; 120 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/lp_filter.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/5/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | class LowPassFilter { 45 | public: 46 | explicit LowPassFilter(ros::NodeHandle &nh); 47 | explicit LowPassFilter(double cutoff_freq); 48 | void input(double in); 49 | void input(double in, ros::Time time); 50 | double output(); 51 | void reset(); 52 | 53 | private: 54 | double in_[3]{}; 55 | double out_[3]{}; 56 | 57 | // Cutoff frequency for the derivative calculation in Hz. 58 | // Negative -> Has not been set by the user yet, so use a default. 59 | double cutoff_frequency_ = -1; 60 | // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency 61 | // at 1/4 of the sample rate. 62 | double c_ = 1.; 63 | // Used to check for tan(0)==>NaN in the filter calculation 64 | double tan_filt_ = 1.; 65 | bool is_debug_{}; 66 | 67 | ros::Time prev_time_; 68 | ros::Duration delta_t_; 69 | 70 | std::shared_ptr> realtime_pub_{}; 71 | }; 72 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/filters/one_euro_filter.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 liucong on 2020/12/5. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | 42 | template 43 | class OneEuroFilter 44 | { 45 | public: 46 | OneEuroFilter(double _freq, T _mincutoff, T _beta, T _dcutoff) 47 | : freq(_freq), mincutoff(_mincutoff), beta(_beta), dcutoff(_dcutoff) 48 | { 49 | firsttime = true; 50 | x_prev = 0; 51 | hatxprev = 0; 52 | dhatxprev = 0; 53 | filtered_val = 0; 54 | }; 55 | 56 | ~OneEuroFilter() = default; 57 | 58 | void input(T input_value) 59 | { 60 | T dx = 0; 61 | if (!firsttime) 62 | dx = (input_value - x_prev) * freq; 63 | if (firsttime) 64 | dhatxprev = dx; 65 | T edx = alpha(dcutoff, freq) * dx + (1 - alpha(dcutoff, freq)) * dhatxprev; 66 | dhatxprev = edx; 67 | T cutoff = mincutoff + beta * std::abs(static_cast(edx)); 68 | 69 | if (firsttime) 70 | hatxprev = input_value; 71 | filtered_val = alpha(cutoff, freq) * input_value + (1 - alpha(cutoff, freq)) * hatxprev; 72 | hatxprev = filtered_val; 73 | firsttime = false; 74 | }; 75 | 76 | T alpha(T cutoff, double freq) 77 | { 78 | T tau = 1.0 / (2 * M_PI * cutoff); 79 | T te = 1.0 / freq; 80 | return 1.0 / (1.0 + tau / te); 81 | } 82 | 83 | T output() 84 | { 85 | return filtered_val; 86 | }; 87 | 88 | void clear() 89 | { 90 | firsttime = true; 91 | x_prev = 0; 92 | hatxprev = 0; 93 | dhatxprev = 0; 94 | }; 95 | 96 | private: 97 | double freq; 98 | bool firsttime; 99 | T mincutoff, beta, dcutoff; 100 | T x_prev, dhatxprev, hatxprev; 101 | T filtered_val; 102 | }; 103 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/action_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace rc_control 10 | { 11 | struct ActionData 12 | { 13 | std::string name; 14 | double yaw_angle; 15 | double pitch_angle; 16 | double roll_angle; 17 | double pose_x; 18 | double pose_y; 19 | double yaw_acc; 20 | }; 21 | 22 | struct ActionCmd 23 | { 24 | bool calibration_state; 25 | bool reset_state; 26 | bool update_x_state; 27 | double update_x; 28 | bool update_y_state; 29 | double update_y; 30 | bool update_yaw_state; 31 | double update_yaw; 32 | }; 33 | 34 | class ActionHandle 35 | { 36 | public: 37 | ActionHandle() = default; 38 | ActionHandle(std::string name, ActionData* action_data, ActionCmd* action_cmd) 39 | : name_(std::move(name)), action_data_(action_data), action_cmd_(action_cmd) 40 | { 41 | if (!action_data_) 42 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 43 | "'. action_state pointer is null."); 44 | if (!action_cmd_) 45 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 46 | "'. action_cmd pointer is null."); 47 | } 48 | 49 | std::string getName() const 50 | { 51 | assert(action_data_); 52 | return action_data_->name; 53 | } 54 | double getYawAngle() const 55 | { 56 | assert(action_data_); 57 | return action_data_->yaw_angle; 58 | } 59 | double getPitchAngle() const 60 | { 61 | assert(action_data_); 62 | return action_data_->pitch_angle; 63 | } 64 | double getRollAngle() const 65 | { 66 | assert(action_data_); 67 | return action_data_->roll_angle; 68 | } 69 | double getPoseX() const 70 | { 71 | assert(action_data_); 72 | return action_data_->pose_x; 73 | } 74 | double getPoseY() const 75 | { 76 | assert(action_data_); 77 | return action_data_->pose_y; 78 | } 79 | double getYawAcc() const 80 | { 81 | assert(action_data_); 82 | return action_data_->yaw_acc; 83 | } 84 | 85 | bool getCalibrationState() const 86 | { 87 | assert(action_cmd_); 88 | return action_cmd_->calibration_state; 89 | } 90 | bool getResetState() const 91 | { 92 | assert(action_cmd_); 93 | return action_cmd_->reset_state; 94 | } 95 | bool getUpdateXState() const 96 | { 97 | assert(action_cmd_); 98 | return action_cmd_->update_x_state; 99 | } 100 | bool getUpdateYState() const 101 | { 102 | assert(action_cmd_); 103 | return action_cmd_->update_y_state; 104 | } 105 | bool getUpdateYawState() const 106 | { 107 | assert(action_cmd_); 108 | return action_cmd_->update_yaw_state; 109 | } 110 | 111 | void setCalibration() 112 | { 113 | // calibration_state default is true; 114 | action_cmd_->calibration_state = false; 115 | } 116 | void resetAction() const 117 | { 118 | action_cmd_->reset_state = true; 119 | } 120 | void updatePoseX(double update_x) 121 | { 122 | action_cmd_->update_x_state = true; 123 | action_cmd_->update_x = update_x; 124 | } 125 | void updatePoseY(double update_y) 126 | { 127 | action_cmd_->update_y_state = true; 128 | action_cmd_->update_y = update_y; 129 | } 130 | void updatePoseYaw(double update_yaw) 131 | { 132 | action_cmd_->update_yaw_state = true; 133 | action_cmd_->update_yaw = update_yaw; 134 | } 135 | 136 | private: 137 | std::string name_; 138 | ActionData* action_data_; 139 | ActionCmd* action_cmd_; 140 | }; 141 | 142 | class ActionInterface 143 | : public hardware_interface::HardwareResourceManager 144 | { 145 | }; 146 | 147 | } // namespace rc_control -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/actuator_extra_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | 11 | namespace rc_control 12 | { 13 | class ActuatorExtraHandle 14 | { 15 | public: 16 | ActuatorExtraHandle() = default; 17 | ActuatorExtraHandle(std::string name, bool* halted, bool* need_calibration, bool* calibrated, 18 | bool* calibration_reading, double* pos, double* offset) 19 | : name_(std::move(name)) 20 | , halted_(halted) 21 | , need_calibration_(need_calibration) 22 | , calibrated_(calibrated) 23 | , calibration_reading_(calibration_reading) 24 | , pos_(pos) 25 | , offset_(offset) 26 | { 27 | if (!halted) 28 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 29 | "'. halted pointer is null."); 30 | if (!need_calibration) 31 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 32 | "'. need_calibration pointer is null."); 33 | if (!calibrated) 34 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 35 | "'. calibrated pointer is null."); 36 | if (!calibration_reading) 37 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 38 | "'. calibration reading pointer is null."); 39 | if (!pos) 40 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + "'. pos pointer is null."); 41 | if (!offset) 42 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 43 | "'. offset pointer is null."); 44 | } 45 | std::string getName() const 46 | { 47 | return name_; 48 | } 49 | bool getHalted() const 50 | { 51 | assert(halted_); 52 | return *halted_; 53 | } 54 | bool getNeedCalibration() const 55 | { 56 | assert(need_calibration_); 57 | return *need_calibration_; 58 | } 59 | bool getCalibrated() const 60 | { 61 | assert(calibrated_); 62 | return *calibrated_; 63 | } 64 | bool getCalibrationReading() const 65 | { 66 | assert(calibration_reading_); 67 | return *calibration_reading_; 68 | } 69 | double getPosition() const 70 | { 71 | assert(pos_); 72 | return *pos_; 73 | } 74 | double getOffset() const 75 | { 76 | assert(offset_); 77 | return *offset_; 78 | } 79 | void setOffset(double offset) 80 | { 81 | *offset_ = offset; 82 | } 83 | void setCalibrated(bool calibrated) 84 | { 85 | *calibrated_ = calibrated; 86 | } 87 | 88 | private: 89 | std::string name_; 90 | bool* halted_ = { nullptr }; 91 | bool* need_calibration_ = { nullptr }; 92 | bool* calibrated_ = { nullptr }; 93 | bool* calibration_reading_ = { nullptr }; 94 | double* pos_{}; 95 | double* offset_{}; 96 | }; 97 | 98 | class ActuatorExtraInterface 99 | : public hardware_interface::HardwareResourceManager 100 | { 101 | }; 102 | 103 | } // namespace rc_control 104 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/dt35_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jialonglong on 23-7-18. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace rc_control 10 | { 11 | struct SharpIR 12 | { 13 | std::string name; 14 | double data; 15 | }; 16 | 17 | class SharpIRHandle 18 | { 19 | public: 20 | SharpIRHandle()=default; 21 | SharpIRHandle(std::string name,SharpIR* sharp_data) 22 | : name_(std::move(name)),sharp_data_(sharp_data) 23 | { 24 | if (!sharp_data) 25 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 26 | "'. dt35 pointer is null."); 27 | } 28 | 29 | std::string getName() const 30 | { 31 | assert(sharp_data_); 32 | return sharp_data_->name; 33 | } 34 | 35 | double getSharpIRdata() const 36 | { 37 | assert(sharp_data_); 38 | return sharp_data_->data; 39 | } 40 | private: 41 | std::string name_; 42 | SharpIR* sharp_data_; 43 | protected: 44 | 45 | }; 46 | 47 | class SharpIRInterface 48 | : public hardware_interface::HardwareResourceManager 49 | { 50 | }; 51 | } // namespace rc_control 52 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/gpio_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace rc_control 10 | { 11 | enum GpioType 12 | { 13 | INPUT, 14 | OUTPUT 15 | }; 16 | 17 | struct GpioData 18 | { 19 | std::string name; 20 | GpioType type; 21 | int pin; 22 | bool* value; 23 | }; 24 | 25 | class GpioStateHandle 26 | { 27 | public: 28 | GpioStateHandle() = default; 29 | GpioStateHandle(std::string name, GpioType type, bool* value) : name_(std::move(name)), type_(type), value_(value) 30 | { 31 | if (!value) 32 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 33 | "'. value pointer is null."); 34 | } 35 | std::string getName() const 36 | { 37 | return name_; 38 | } 39 | 40 | GpioType getType() const 41 | { 42 | return type_; 43 | } 44 | 45 | bool getValue() const 46 | { 47 | assert(value_); 48 | return *value_; 49 | } 50 | 51 | private: 52 | std::string name_; 53 | GpioType type_; 54 | bool* value_ = { nullptr }; 55 | }; 56 | 57 | class GpioCommandHandle 58 | { 59 | public: 60 | GpioCommandHandle() = default; 61 | GpioCommandHandle(std::string name, GpioType type, bool* cmd) : name_(std::move(name)), type_(type), cmd_(cmd) 62 | { 63 | if (!cmd) 64 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 65 | "'. command pointer is null."); 66 | } 67 | std::string getName() const 68 | { 69 | return name_; 70 | } 71 | bool getCommand() const 72 | { 73 | assert(cmd_); 74 | return *cmd_; 75 | } 76 | 77 | void setCommand(bool value) 78 | { 79 | assert(cmd_); 80 | *cmd_ = value; 81 | } 82 | 83 | private: 84 | std::string name_; 85 | GpioType type_; 86 | bool* cmd_ = { nullptr }; 87 | }; 88 | 89 | class GpioStateInterface 90 | : public hardware_interface::HardwareResourceManager 91 | { 92 | }; 93 | 94 | class GpioCommandInterface 95 | : public hardware_interface::HardwareResourceManager 96 | { 97 | }; 98 | 99 | } // namespace rc_control -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/rc_imu_sensor_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | namespace rc_control 11 | { 12 | class RcImuSensorHandle : public hardware_interface::ImuSensorHandle 13 | { 14 | public: 15 | RcImuSensorHandle() = default; 16 | 17 | RcImuSensorHandle(const hardware_interface::ImuSensorHandle& imu_sensor_handle, ros::Time* time_stamp) 18 | : ImuSensorHandle(imu_sensor_handle), time_stamp_(time_stamp) 19 | { 20 | if (!time_stamp_) 21 | { 22 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + imu_sensor_handle.getName() + 23 | "'. Time stamp pointer is null"); 24 | } 25 | } 26 | ros::Time getTimeStamp() 27 | { 28 | assert(time_stamp_); 29 | return *time_stamp_; 30 | } 31 | 32 | private: 33 | ros::Time* time_stamp_ = { nullptr }; 34 | }; 35 | 36 | class RcImuSensorInterface 37 | : public hardware_interface::HardwareResourceManager 38 | { 39 | }; 40 | } // namespace rc_control 41 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/hardware_interface/robot_state_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "rc_common/tf_rt_broadcaster.h" 15 | 16 | namespace rc_control 17 | { 18 | class RobotStateHandle 19 | { 20 | public: 21 | RobotStateHandle() = default; 22 | RobotStateHandle(std::string name, tf2_ros::Buffer* buffer) : name_(std::move(name)), buffer_(buffer) 23 | { 24 | if (!buffer) 25 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + 26 | "'. Tf Buffer data pointer is null."); 27 | }; 28 | 29 | geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, 30 | const ros::Time& time) 31 | { 32 | return buffer_->lookupTransform(target_frame, source_frame, time); 33 | } 34 | 35 | bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string& authority, 36 | bool is_static = false) const 37 | { 38 | return buffer_->setTransform(transform, authority, is_static); 39 | } 40 | 41 | bool setTransform(const std::vector& transforms, const std::string& authority, 42 | bool is_static = false) const 43 | { 44 | for (const auto& transform : transforms) 45 | buffer_->setTransform(transform, authority, is_static); 46 | return true; 47 | } 48 | 49 | std::string getName() const 50 | { 51 | return name_; 52 | } 53 | 54 | private: 55 | std::string name_; 56 | tf2_ros::Buffer* buffer_{}; 57 | }; 58 | 59 | class RobotStateInterface 60 | : public hardware_interface::HardwareResourceManager 61 | { 62 | }; 63 | } // namespace rc_control 64 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/linear_interpolation.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 22-5-21. 3 | // 4 | 5 | #pragma once 6 | #include 7 | #include 8 | 9 | namespace rc_common { 10 | class LinearInterp { 11 | public: 12 | LinearInterp() = default; 13 | void init(XmlRpc::XmlRpcValue &config) { 14 | ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeArray); 15 | for (int i = 0; i < config.size(); i++) { 16 | ROS_ASSERT(config[i].getType() == XmlRpc::XmlRpcValue::TypeArray); 17 | ROS_ASSERT(config[i].size() == 2); 18 | ROS_ASSERT(config[i][0].getType() == XmlRpc::XmlRpcValue::TypeDouble || 19 | config[i][0].getType() == XmlRpc::XmlRpcValue::TypeInt); 20 | ROS_ASSERT(config[i][1].getType() == XmlRpc::XmlRpcValue::TypeDouble || 21 | config[i][1].getType() == XmlRpc::XmlRpcValue::TypeInt); 22 | if (!input_vector_.empty()) 23 | if ((double) config[i][0] < input_vector_.back()) { 24 | ROS_ERROR("Please sort the point's abscissa from smallest to largest. %lf < %lf", (double) config[i][0], 25 | input_vector_.back()); 26 | return; 27 | } 28 | input_vector_.push_back(config[i][0]); 29 | output_vector_.push_back(config[i][1]); 30 | } 31 | } 32 | double output(double input) { 33 | if (input >= input_vector_.back()) 34 | return output_vector_.back(); 35 | else if (input <= input_vector_.front()) 36 | return output_vector_.front(); 37 | for (size_t i = 0; i < input_vector_.size(); i++) { 38 | if (input >= input_vector_[i] && input <= input_vector_[i + 1]) 39 | return output_vector_[i] + 40 | ((output_vector_[i + 1] - output_vector_[i]) / (input_vector_[i + 1] - input_vector_[i])) * 41 | (input - input_vector_[i]); 42 | } 43 | ROS_ERROR("The point's abscissa aren't sorted from smallest to largest."); 44 | return 0; 45 | } 46 | 47 | private: 48 | std::vector input_vector_; 49 | std::vector output_vector_; 50 | }; 51 | }// namespace rc_common 52 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/lqr.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 chenzheng on 3/20/21. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include "eigen_types.h" 43 | 44 | template 45 | class Lqr 46 | { 47 | public: 48 | template 49 | Lqr(const Eigen::MatrixBase& A, const Eigen::MatrixBase& B, const Eigen::MatrixBase& Q, 50 | const Eigen::MatrixBase& R) 51 | : a_(A), b_(B), q_(Q), r_(R) 52 | { 53 | // check A 54 | static_assert(TA::RowsAtCompileTime == TA::ColsAtCompileTime, "lqr: A should be square matrix"); 55 | // check B 56 | static_assert(TB::RowsAtCompileTime == TA::RowsAtCompileTime, "lqr: B rows should be equal to A rows"); 57 | // check Q 58 | static_assert(TQ::RowsAtCompileTime == TA::RowsAtCompileTime && TQ::ColsAtCompileTime == TA::ColsAtCompileTime, 59 | "lqr: The rows and columns of Q should be equal to A"); 60 | // check R 61 | static_assert(TR::RowsAtCompileTime == TB::ColsAtCompileTime && TR::ColsAtCompileTime == TB::ColsAtCompileTime, 62 | "lqr: The rows and columns of R should be equal to the cols of B"); 63 | 64 | k_.resize(TB::ColsAtCompileTime, TB::RowsAtCompileTime); 65 | k_.setZero(); 66 | } 67 | 68 | bool solveRiccatiArimotoPotter(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, 69 | const Eigen::MatrixXd& R, Eigen::MatrixXd& P) 70 | { 71 | int dim_x = A.rows(); 72 | 73 | // set Hamilton matrix 74 | Eigen::MatrixXd ham = Eigen::MatrixXd::Zero(2 * dim_x, 2 * dim_x); 75 | ham << A, -B * R.inverse() * B.transpose(), -Q, -A.transpose(); 76 | 77 | // calc eigenvalues and eigenvectors 78 | Eigen::EigenSolver eigs(ham); 79 | 80 | // extract stable eigenvectors into 'eigvec' 81 | Eigen::MatrixXcd eigvec = Eigen::MatrixXcd::Zero(2 * dim_x, dim_x); 82 | int j = 0; 83 | for (int i = 0; i < 2 * dim_x; ++i) 84 | { 85 | if (eigs.eigenvalues()[i].real() < 0.) 86 | { 87 | eigvec.col(j) = eigs.eigenvectors().block(0, i, 2 * dim_x, 1); 88 | ++j; 89 | } 90 | } 91 | 92 | // calc P with stable eigen vector matrix 93 | Eigen::MatrixXcd vs_1, vs_2; 94 | vs_1 = eigvec.block(0, 0, dim_x, dim_x); 95 | vs_2 = eigvec.block(dim_x, 0, dim_x, dim_x); 96 | P = (vs_2 * vs_1.inverse()).real(); 97 | 98 | return true; 99 | } 100 | 101 | bool computeK() 102 | { 103 | Eigen::SelfAdjointEigenSolver > q_solver(q_); 104 | Eigen::SelfAdjointEigenSolver > r_solver(r_); 105 | if (q_solver.info() != Eigen::Success || r_solver.info() != Eigen::Success) 106 | return false; 107 | // q (r) must be symmetric positive (semi) definite 108 | for (int i = 0; i < q_solver.eigenvalues().cols(); ++i) 109 | { 110 | if (q_solver.eigenvalues()[i] < 0) 111 | return false; 112 | } 113 | for (int i = 0; i < r_solver.eigenvalues().cols(); ++i) 114 | { 115 | if (r_solver.eigenvalues()[i] <= 0) 116 | return false; 117 | } 118 | if (!isSymmetric(q_) || !isSymmetric(r_)) 119 | return false; 120 | 121 | DMat p; 122 | if (solveRiccatiArimotoPotter(a_, b_, q_, r_, p)) 123 | k_ = r_.inverse() * (b_.transpose() * p.transpose()); 124 | return true; 125 | } 126 | 127 | Eigen::Matrix getK() 128 | { 129 | return k_; 130 | } 131 | 132 | private: 133 | bool isSymmetric(DMat m) 134 | { 135 | for (int i = 0; i < m.rows() - 1; ++i) 136 | { 137 | for (int j = i + 1; j < m.cols(); ++j) 138 | { 139 | if (m(i, j - i) != m(j - i, i)) 140 | return false; 141 | } 142 | } 143 | return true; 144 | } 145 | 146 | DMat a_, b_, q_, r_, k_; 147 | }; 148 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/math_utilities.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 12/22/19. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | template 42 | T angularMinus(T a, T b) 43 | { 44 | a = fmod(a, 2.0 * M_PI); 45 | b = fmod(b, 2.0 * M_PI); 46 | 47 | T res1 = a - b; 48 | T res2 = (a < b) ? (a + 2 * M_PI - b) : (a - 2 * M_PI - b); 49 | 50 | return (std::abs(res1) < std::abs(res2)) ? res1 : res2; 51 | } 52 | 53 | template 54 | T minAbs(T a, T b) 55 | { 56 | T sign = (a < 0.0) ? -1.0 : 1.0; 57 | return sign * fmin(fabs(a), b); 58 | } 59 | 60 | template 61 | int sgn(T val) 62 | { 63 | return (T(0) < val) - (val < T(0)); 64 | } 65 | 66 | template 67 | T square(T val) 68 | { 69 | return val * val; 70 | } 71 | 72 | template 73 | T alpha(T cutoff, double freq) 74 | { 75 | T tau = 1.0 / (2 * M_PI * cutoff); 76 | T te = 1.0 / freq; 77 | return 1.0 / (1.0 + tau / te); 78 | } 79 | 80 | template 81 | T angToRad(T ang) 82 | { 83 | return ang * M_PI / 180.0; 84 | } 85 | 86 | template 87 | T radToAng(T rad) 88 | { 89 | return rad * 180.0 / M_PI; 90 | } -------------------------------------------------------------------------------- /rc_common/include/rc_common/ori_tool.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/13/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /*! 45 | * Convert a quaternion to RPY. Uses ZYX order (yaw-pitch-roll), but returns 46 | * angles in (roll, pitch, yaw). 47 | */ 48 | void quatToRPY(const geometry_msgs::Quaternion& q, double& roll, double& pitch, double& yaw); 49 | 50 | double yawFromQuat(const geometry_msgs::Quaternion& q); 51 | 52 | tf::Quaternion getAverageQuaternion(const std::vector& quaternions, const std::vector& weights); 53 | 54 | tf::Quaternion rotationMatrixToQuaternion(const Eigen::Map& rot); 55 | 56 | geometry_msgs::Quaternion rpyToQuat(const double& roll, const double& pitch, const double& yaw); -------------------------------------------------------------------------------- /rc_common/include/rc_common/ros_utilities.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 7/7/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | 43 | template 44 | T getParam(ros::NodeHandle& pnh, const std::string& param_name, const T& default_val) 45 | { 46 | T param_val; 47 | pnh.param(param_name, param_val, default_val); 48 | return param_val; 49 | } 50 | 51 | inline double xmlRpcGetDouble(XmlRpc::XmlRpcValue& value) 52 | { 53 | if (value.getType() == XmlRpc::XmlRpcValue::TypeInt) 54 | { 55 | const int tmp = value; 56 | return (double)tmp; 57 | } 58 | else 59 | return value; 60 | } 61 | 62 | inline double xmlRpcGetDouble(XmlRpc::XmlRpcValue& value, int field) 63 | { 64 | ROS_ASSERT((value[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) || 65 | (value[field].getType() == XmlRpc::XmlRpcValue::TypeInt)); 66 | XmlRpc::XmlRpcValue value_xml = value[field]; 67 | return xmlRpcGetDouble(value[field]); 68 | } 69 | 70 | inline double xmlRpcGetDouble(XmlRpc::XmlRpcValue& value, const std::string& field, double default_value) 71 | { 72 | if (value.hasMember(field)) 73 | { 74 | ROS_ASSERT((value[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) || 75 | (value[field].getType() == XmlRpc::XmlRpcValue::TypeInt)); 76 | return xmlRpcGetDouble(value[field]); 77 | } 78 | else 79 | return default_value; 80 | } 81 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/tf_rt_broadcaster.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 | #include 43 | #include 44 | 45 | namespace rc_common { 46 | class TfRtBroadcaster { 47 | public: 48 | TfRtBroadcaster() = default; 49 | virtual void init(ros::NodeHandle &root_nh); 50 | virtual void sendTransform(const geometry_msgs::TransformStamped &transform); 51 | virtual void sendTransform(const std::vector &transforms); 52 | 53 | protected: 54 | ros::NodeHandle node_; 55 | std::shared_ptr> realtime_pub_{}; 56 | }; 57 | 58 | class StaticTfRtBroadcaster : public TfRtBroadcaster { 59 | public: 60 | void init(ros::NodeHandle &root_nh) override; 61 | void sendTransform(const geometry_msgs::TransformStamped &transform) override; 62 | void sendTransform(const std::vector &transforms) override; 63 | 64 | private: 65 | tf2_msgs::TFMessage net_message_{}; 66 | }; 67 | 68 | }// namespace rc_common 69 | -------------------------------------------------------------------------------- /rc_common/include/rc_common/traj_gen.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 3/21/20. 36 | // 37 | 38 | #pragma once 39 | 40 | #include 41 | #include 42 | #include "math_utilities.h" 43 | 44 | template 45 | class RampTraj 46 | { 47 | private: 48 | T start_{}; // Start pos and velocity 49 | T target_{}; // End pos and velocity 50 | T time_total_{}; 51 | T time_acc_{}; 52 | T time_start_{}; 53 | 54 | T acc_{}; 55 | T speed_{}; // When in uniform speed 56 | T a0_, b0_, c0_; 57 | T a1_, b1_, c1_; 58 | 59 | public: 60 | RampTraj() = default; 61 | void setLimit(T max_acc) 62 | { 63 | acc_ = max_acc; 64 | } 65 | void setState(T start, T end, T time_now) 66 | { 67 | start_ = start; 68 | target_ = end; 69 | time_start_ = time_now; 70 | }; 71 | bool isReach(T t) 72 | { 73 | return (t >= time_total_ + time_start_); 74 | } 75 | 76 | bool calc(T t) 77 | { 78 | if ((target_ - start_) < 0) 79 | { 80 | acc_ = -acc_; 81 | } 82 | time_total_ = t; 83 | if (abs(acc_) // if distant is too small 84 | < abs(4. * (target_ - start_)) / (t * t)) 85 | { 86 | return false; 87 | } 88 | time_acc_ = time_total_ / 2. - 89 | sqrt(acc_ * acc_ * time_total_ * time_total_ - 4. * acc_ * (target_ - start_)) / abs(2. * acc_); 90 | a0_ = start_; 91 | b0_ = 0.; 92 | c0_ = 0.5 * acc_; 93 | a1_ = target_ - (acc_ * time_total_ * time_total_) / 2.; 94 | b1_ = acc_ * time_total_; 95 | c1_ = -0.5 * acc_; 96 | speed_ = b0_ + 2. * c0_ * time_acc_; 97 | return true; 98 | }; 99 | 100 | T getPos(T t) 101 | { 102 | t -= time_start_; 103 | if (t < 0.) 104 | return start_; 105 | else if (t > time_total_) 106 | return target_; 107 | 108 | if (t <= time_total_) 109 | { 110 | if (t < time_acc_) 111 | return a0_ + b0_ * t + c0_ * t * t; 112 | else if (t < time_total_ - time_acc_) 113 | return speed_ * (t - time_acc_) + a0_ + c0_ * (time_acc_ * time_acc_); 114 | else 115 | return a1_ + b1_ * t + c1_ * t * t; 116 | } 117 | else 118 | return target_; 119 | } 120 | 121 | T getVel(T t) 122 | { 123 | t -= time_start_; 124 | if (t < 0. || t > time_total_) 125 | return 0; 126 | 127 | if (t <= time_total_) 128 | { 129 | if (t < time_acc_) 130 | return b0_ + 2. * c0_ * t; 131 | else if (t < time_total_ - time_acc_) 132 | return speed_; 133 | else 134 | return b1_ + 2. * c1_ * t; 135 | } 136 | else 137 | return 0.; 138 | } 139 | 140 | T getAcc(T t) 141 | { 142 | t -= time_start_; 143 | if (t < 0. || t > time_total_) 144 | return 0; 145 | 146 | if (t <= time_total_) 147 | { 148 | if (t < time_acc_) 149 | return 2. * c0_; 150 | else if (t < time_total_ - time_acc_) 151 | return 0; 152 | else 153 | return 2. * c1_; 154 | } 155 | else 156 | return 0.; 157 | } 158 | }; 159 | 160 | // actually it is a controller 161 | // ref: https://build-its-inprogress.blogspot.com/2017/12/controls-ramblings-how-to-get-from.html 162 | template 163 | class MinTimeTraj 164 | { 165 | private: 166 | T target_{}; 167 | T inertia_{}; 168 | T max_tau_{}; 169 | T tolerance_{}; 170 | bool is_reach_{}; 171 | 172 | public: 173 | MinTimeTraj() = default; 174 | void setLimit(T max_tau, T inertia, T tolerance) 175 | { 176 | max_tau_ = max_tau; 177 | inertia_ = inertia; 178 | tolerance_ = tolerance; 179 | } 180 | 181 | void setTarget(T target) 182 | { 183 | target_ = target; 184 | is_reach_ = false; 185 | } 186 | bool isReach() 187 | { 188 | return is_reach_; 189 | } 190 | T getTau(T pos, T vel) 191 | { 192 | T dx = pos - target_; 193 | if (std::abs(dx) > tolerance_) 194 | return max_tau_ * sgn(-max_tau_ * dx - 0.5 * inertia_ * vel * std::abs(vel)); 195 | else 196 | { 197 | is_reach_ = true; 198 | return 0; 199 | } 200 | } 201 | }; 202 | -------------------------------------------------------------------------------- /rc_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rc_common 4 | 0.0.0 5 | The rc_common package 6 | 7 | 8 | 9 | 10 | myx 11 | 12 | 13 | BSD 14 | 15 | catkin 16 | roscpp 17 | roslint 18 | 19 | tf 20 | geometry_msgs 21 | realtime_tools 22 | rc_msgs 23 | imu_complementary_filter 24 | imu_filter_madgwick 25 | dynamic_reconfigure 26 | eigen 27 | control_msgs 28 | controller_manager_msgs 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /rc_common/src/filter/imu_complementary_filter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2022/3/26. 3 | // 4 | 5 | #include "rc_common/filters/imu_complementary_filter.h" 6 | 7 | namespace rc_common { 8 | void ImuComplementaryFilter::filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) { 9 | filter_->update(ax, ay, az, wx, wy, wz, dt); 10 | } 11 | void ImuComplementaryFilter::getOrientation(double &q0, double &q1, double &q2, double &q3) { 12 | filter_->getOrientation(q0, q1, q2, q3); 13 | } 14 | bool ImuComplementaryFilter::initFilter(XmlRpc::XmlRpcValue &imu_data) { 15 | use_mag_ = imu_data.hasMember("use_mag") && (bool) imu_data["use_mag"]; 16 | gain_acc_ = imu_data.hasMember("gain_acc") ? (double) imu_data["gain_acc"] : 0.01; 17 | gain_mag_ = imu_data.hasMember("gain_mag") ? (double) imu_data["gain_mag"] : 0.01; 18 | do_bias_estimation_ = !imu_data.hasMember("do_bias_estimation") || (bool) imu_data["do_bias_estimation"]; 19 | bias_alpha_ = imu_data.hasMember("bias_alpha") ? (double) imu_data["bias_alpha"] : 0.01; 20 | do_adaptive_gain_ = !imu_data.hasMember("do_adaptive_gain") || (bool) imu_data["do_adaptive_gain"]; 21 | resetFilter(); 22 | return true; 23 | } 24 | void ImuComplementaryFilter::resetFilter() { 25 | filter_ = std::make_shared(); 26 | filter_->setDoBiasEstimation(do_bias_estimation_); 27 | filter_->setDoAdaptiveGain(do_adaptive_gain_); 28 | if (!filter_->setGainAcc(gain_acc_)) 29 | ROS_WARN("Invalid gain_acc passed to ComplementaryFilter."); 30 | if (use_mag_) { 31 | if (!filter_->setGainMag(gain_mag_)) 32 | ROS_WARN("Invalid gain_mag passed to ComplementaryFilter."); 33 | } 34 | if (do_bias_estimation_) { 35 | if (!filter_->setBiasAlpha(bias_alpha_)) 36 | ROS_WARN("Invalid bias_alpha passed to ComplementaryFilter."); 37 | } 38 | return; 39 | } 40 | }// namespace rc_common 41 | -------------------------------------------------------------------------------- /rc_common/src/filter/imu_filter_base.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2022/3/26. 3 | // 4 | 5 | #include "rc_common/filters/imu_filter_base.h" 6 | 7 | namespace rc_common { 8 | bool ImuFilterBase::init(XmlRpc::XmlRpcValue &imu_data, const std::string &name) { 9 | ros::NodeHandle nh("~"); 10 | frame_id_ = (std::string) imu_data["frame_id"]; 11 | initFilter(imu_data); 12 | imu_data_pub_ = nh.advertise(name + "/data", 100); 13 | imu_temp_pub_ = nh.advertise(name + "/temperature", 100); 14 | trigger_time_pub_ = nh.advertise(name + "/trigger_time", 100); 15 | return true; 16 | } 17 | 18 | void ImuFilterBase::update(ros::Time time, double *accel, double *omega, double *ori, double *accel_cov, 19 | double *omega_cov, double *ori_cov, double temp, bool camera_trigger) { 20 | if (!initialized_filter_) { 21 | initialized_filter_ = true; 22 | last_update_ = time; 23 | imu_pub_data_.header.frame_id = frame_id_; 24 | return; 25 | } 26 | if ((time - last_update_).toSec() > 1) { 27 | resetFilter(); 28 | last_update_ = time; 29 | return; 30 | } 31 | // Update the filter. 32 | filterUpdate(accel[0], accel[1], accel[2], omega[0], omega[1], omega[2], (time - last_update_).toSec()); 33 | last_update_ = time; 34 | getOrientation(ori[3], ori[0], ori[1], ori[2]); 35 | if (camera_trigger) { 36 | imu_pub_data_.header.stamp = time; 37 | imu_pub_data_.angular_velocity.x = omega[0]; 38 | imu_pub_data_.angular_velocity.y = omega[1]; 39 | imu_pub_data_.angular_velocity.z = omega[2]; 40 | imu_pub_data_.linear_acceleration.x = accel[0]; 41 | imu_pub_data_.linear_acceleration.y = accel[1]; 42 | imu_pub_data_.linear_acceleration.z = accel[2]; 43 | imu_pub_data_.orientation.x = ori[0]; 44 | imu_pub_data_.orientation.y = ori[1]; 45 | imu_pub_data_.orientation.z = ori[2]; 46 | imu_pub_data_.orientation.w = ori[3]; 47 | imu_pub_data_.orientation_covariance = {ori_cov[0], 0., 0., 0., ori_cov[4], 0., 0., 0., ori_cov[8]}; 48 | imu_pub_data_.angular_velocity_covariance = {omega_cov[0], 0., 0., 0., omega_cov[4], 0., 0., 0., omega_cov[8]}; 49 | imu_pub_data_.linear_acceleration_covariance = {accel_cov[0], 0., 0., 0., accel_cov[4], 0., 0., 0., accel_cov[8]}; 50 | imu_data_pub_.publish(imu_pub_data_); 51 | 52 | trigger_time_pub_data_.header.stamp = time; 53 | trigger_time_pub_data_.time_ref = time; 54 | trigger_time_pub_.publish(trigger_time_pub_data_); 55 | 56 | temp_pub_data_.header.stamp = time; 57 | temp_pub_data_.temperature = temp; 58 | imu_temp_pub_.publish(temp_pub_data_); 59 | } 60 | } 61 | 62 | }// namespace rc_common 63 | -------------------------------------------------------------------------------- /rc_common/src/filter/lp_filter.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 1/5/21. 36 | // 37 | 38 | #include "rc_common/filters/lp_filter.h" 39 | #include "rc_common/ros_utilities.h" 40 | 41 | LowPassFilter::LowPassFilter(ros::NodeHandle &nh) { 42 | nh.param("lp_cutoff_frequency", cutoff_frequency_, -1.); 43 | nh.param("lp_debug", is_debug_, false); 44 | 45 | if (is_debug_) 46 | realtime_pub_.reset(new realtime_tools::RealtimePublisher(nh, "lp_filter", 100)); 47 | } 48 | 49 | LowPassFilter::LowPassFilter(double cutoff_freq) { 50 | is_debug_ = false; 51 | cutoff_frequency_ = cutoff_freq; 52 | } 53 | 54 | void LowPassFilter::input(double in, ros::Time time) { 55 | // My filter reference was Julius O. Smith III, Intro. to Digital Filters 56 | // With Audio Applications. 57 | // See https://ccrma.stanford.edu/~jos/filters/Example_Second_Order_Butterworth_Lowpass.html 58 | in_[2] = in_[1]; 59 | in_[1] = in_[0]; 60 | in_[0] = in; 61 | 62 | if (!prev_time_.isZero())// Not first time through the program 63 | { 64 | delta_t_ = time - prev_time_; 65 | prev_time_ = time; 66 | if (0 == delta_t_.toSec()) { 67 | ROS_ERROR("delta_t is 0, skipping this loop. Possible overloaded cpu " 68 | "at time: %f", 69 | time.toSec()); 70 | return; 71 | } 72 | } else { 73 | prev_time_ = time; 74 | return; 75 | } 76 | 77 | if (cutoff_frequency_ != -1 && cutoff_frequency_ > 0) { 78 | // Check if tan(_) is really small, could cause c = NaN 79 | tan_filt_ = tan((cutoff_frequency_ * 6.2832) * delta_t_.toSec() / 2.); 80 | // Avoid tan(0) ==> NaN 81 | if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) 82 | tan_filt_ = -0.01; 83 | if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) 84 | tan_filt_ = 0.01; 85 | c_ = 1 / tan_filt_; 86 | } 87 | 88 | out_[2] = out_[1]; 89 | out_[1] = out_[0]; 90 | out_[0] = (1 / (1 + c_ * c_ + M_SQRT2 * c_)) * 91 | (in_[2] + 2 * in_[1] + in_[0] - (c_ * c_ - M_SQRT2 * c_ + 1) * out_[2] - (-2 * c_ * c_ + 2) * out_[1]); 92 | 93 | if (is_debug_) { 94 | if (realtime_pub_->trylock()) { 95 | realtime_pub_->msg_.header.stamp = time; 96 | realtime_pub_->msg_.real = in_[0]; 97 | realtime_pub_->msg_.filtered = out_[0]; 98 | realtime_pub_->unlockAndPublish(); 99 | } 100 | } 101 | } 102 | 103 | void LowPassFilter::input(double in) { 104 | input(in, ros::Time::now()); 105 | } 106 | 107 | double LowPassFilter::output() { 108 | return out_[0]; 109 | } 110 | 111 | void LowPassFilter::reset() { 112 | for (int i = 0; i < 3; ++i) { 113 | in_[i] = 0; 114 | out_[i] = 0; 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /rc_common/src/ori_tool.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 8/13/20. 36 | // 37 | 38 | #include "rc_common/ori_tool.h" 39 | #include "rc_common/math_utilities.h" 40 | #include 41 | 42 | void quatToRPY(const geometry_msgs::Quaternion& q, double& roll, double& pitch, double& yaw) 43 | { 44 | double as = std::min(-2. * (q.x * q.z - q.w * q.y), .99999); 45 | yaw = std::atan2(2 * (q.x * q.y + q.w * q.z), square(q.w) + square(q.x) - square(q.y) - square(q.z)); 46 | pitch = std::asin(as); 47 | roll = std::atan2(2 * (q.y * q.z + q.w * q.x), square(q.w) - square(q.x) - square(q.y) + square(q.z)); 48 | } 49 | 50 | double yawFromQuat(const geometry_msgs::Quaternion& q) 51 | { 52 | double roll, pitch, yaw; 53 | quatToRPY(q, roll, pitch, yaw); 54 | return yaw; 55 | } 56 | 57 | tf::Quaternion getAverageQuaternion(const std::vector& quaternions, const std::vector& weights) 58 | { 59 | Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size()); 60 | Eigen::Vector3d vec; 61 | for (size_t i = 0; i < quaternions.size(); ++i) 62 | { 63 | // Weigh the quaternions according to their associated weight 64 | tf::Quaternion quat = quaternions[i] * weights[i]; 65 | // Append the weighted Quaternion to a matrix Q. 66 | Q(0, i) = quat.x(); 67 | Q(1, i) = quat.y(); 68 | Q(2, i) = quat.z(); 69 | Q(3, i) = quat.w(); 70 | } 71 | // Creat a solver for finding the eigenvectors and eigenvalues 72 | Eigen::EigenSolver es(Q * Q.transpose()); 73 | // Find index of maximum (real) Eigenvalue. 74 | auto eigenvalues = es.eigenvalues(); 75 | size_t max_idx = 0; 76 | double max_value = eigenvalues[max_idx].real(); 77 | for (size_t i = 1; i < 4; ++i) 78 | { 79 | double real = eigenvalues[i].real(); 80 | if (real > max_value) 81 | { 82 | max_value = real; 83 | max_idx = i; 84 | } 85 | } 86 | // Get corresponding Eigenvector, normalize it and return it as the average quat 87 | auto eigenvector = es.eigenvectors().col(max_idx).normalized(); 88 | tf::Quaternion mean_orientation(eigenvector[0].real(), eigenvector[1].real(), eigenvector[2].real(), 89 | eigenvector[3].real()); 90 | return mean_orientation; 91 | } 92 | 93 | tf::Quaternion rotationMatrixToQuaternion(const Eigen::Map& rot) 94 | { 95 | Eigen::Matrix3d r = rot.transpose(); 96 | tf::Quaternion quat; 97 | double trace = r.trace(); 98 | if (trace > 0.0) 99 | { 100 | double s = sqrt(trace + 1.0) * 2.0; 101 | quat.setValue((r(2, 1) - r(1, 2)) / s, (r(0, 2) - r(2, 0)) / s, (r(1, 0) - r(0, 1)) / s, 0.25 * s); 102 | } 103 | else if ((r(0, 0) > r(1, 1)) && (r(0, 0) > r(2, 2))) 104 | { 105 | double s = sqrt(1.0 + r(0, 0) - r(1, 1) - r(2, 2)) * 2.0; 106 | quat.setValue(0.25 * s, (r(0, 1) + r(1, 0)) / s, (r(0, 2) + r(2, 0)) / s, (r(2, 1) - r(1, 2)) / s); 107 | } 108 | else if (r(1, 1) > r(2, 2)) 109 | { 110 | double s = sqrt(1.0 + r(1, 1) - r(0, 0) - r(2, 2)) * 2.0; 111 | quat.setValue((r(0, 1) + r(1, 0)) / s, (r(0, 1) + r(1, 0)) / s, 0.25 * s, (r(0, 2) - r(2, 0)) / s); 112 | } 113 | else 114 | { 115 | double s = sqrt(1.0 + r(2, 2) - r(0, 0) - r(1, 1)) * 2.0; 116 | quat.setValue((r(0, 2) + r(2, 0)) / s, (r(1, 2) + r(2, 1)) / s, 0.25 * s, (r(1, 0) - r(0, 1)) / s); 117 | } 118 | return quat; 119 | } 120 | 121 | geometry_msgs::Quaternion rpyToQuat(const double& roll, const double& pitch, const double& yaw) 122 | { 123 | return tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); 124 | } -------------------------------------------------------------------------------- /rc_common/src/tf_rt_broadcaster.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 1/3/21. 36 | // 37 | #include "rc_common/tf_rt_broadcaster.h" 38 | 39 | #include 40 | #include 41 | 42 | namespace rc_common { 43 | void TfRtBroadcaster::init(ros::NodeHandle &root_nh) { 44 | realtime_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); 45 | } 46 | 47 | void TfRtBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) { 48 | std::vector v1; 49 | v1.push_back(transform); 50 | sendTransform(v1); 51 | } 52 | 53 | void TfRtBroadcaster::sendTransform(const std::vector &transforms) { 54 | tf2_msgs::TFMessage message; 55 | for (const auto &transform: transforms) { 56 | message.transforms.push_back(transform); 57 | } 58 | if (realtime_pub_->trylock()) { 59 | realtime_pub_->msg_ = message; 60 | realtime_pub_->unlockAndPublish(); 61 | } 62 | } 63 | 64 | void StaticTfRtBroadcaster::init(ros::NodeHandle &root_nh) { 65 | realtime_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf_static", 100, true)); 66 | } 67 | 68 | void StaticTfRtBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) { 69 | std::vector v1; 70 | v1.push_back(transform); 71 | sendTransform(v1); 72 | } 73 | 74 | void StaticTfRtBroadcaster::sendTransform(const std::vector &transforms) { 75 | for (const auto &transform: transforms) { 76 | bool match_found = false; 77 | for (auto &it_msg: net_message_.transforms) { 78 | if (transform.child_frame_id == it_msg.child_frame_id) { 79 | it_msg = transform; 80 | match_found = true; 81 | break; 82 | } 83 | } 84 | if (!match_found) 85 | net_message_.transforms.push_back(transform); 86 | } 87 | if (realtime_pub_->trylock()) { 88 | realtime_pub_->msg_ = net_message_; 89 | realtime_pub_->unlockAndPublish(); 90 | } 91 | } 92 | 93 | }// namespace rc_common 94 | -------------------------------------------------------------------------------- /rc_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rc_gazebo) 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 | roslint 17 | rc_common 18 | 19 | gazebo_ros 20 | gazebo_ros_control 21 | ) 22 | 23 | # Depend on system install of Gazebo 24 | find_package(gazebo REQUIRED) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 26 | 27 | ################################### 28 | ## catkin specific configuration ## 29 | ################################### 30 | ## The catkin_package macro generates cmake config files for your package 31 | ## Declare things to be passed to dependent projects 32 | ## INCLUDE_DIRS: uncomment this if your package contains header files 33 | ## LIBRARIES: libraries you create in this project that dependent projects also need 34 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 35 | ## DEPENDS: system dependencies of this project that dependent projects also need 36 | catkin_package( 37 | INCLUDE_DIRS 38 | include 39 | LIBRARIES 40 | rc_action_hw_sim 41 | CATKIN_DEPENDS 42 | roscpp 43 | rc_common 44 | 45 | gazebo_ros 46 | gazebo_ros_control 47 | DEPENDS 48 | ) 49 | 50 | ########### 51 | ## Build ## 52 | ########### 53 | 54 | ## Specify additional locations of header files 55 | ## Your package locations should be listed before other locations 56 | include_directories( 57 | include 58 | ${catkin_INCLUDE_DIRS} 59 | ${GAZEBO_INCLUDE_DIRS} 60 | ) 61 | 62 | ## Declare a cpp library 63 | add_library(rc_action_hw_sim 64 | src/rc_action_hw_sim.cpp 65 | ) 66 | 67 | add_dependencies(rc_action_hw_sim 68 | ${catkin_EXPORTED_TARGETS} 69 | ) 70 | 71 | target_link_libraries(rc_action_hw_sim 72 | ${catkin_LIBRARIES} 73 | ${GAZEBO_LIBRARIES} 74 | ) 75 | 76 | ############# 77 | ## Install ## 78 | ############# 79 | 80 | ## Mark executables and/or libraries for installation 81 | install(TARGETS rc_action_hw_sim 82 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 84 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 85 | ) 86 | 87 | # Mark cpp header files for installation 88 | install(DIRECTORY include/${PROJECT_NAME}/ 89 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 90 | FILES_MATCHING PATTERN "*.h" 91 | ) 92 | 93 | # Mark other files for installation 94 | install(DIRECTORY config launch worlds 95 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 96 | ) 97 | install(FILES rc_action_hw_sim_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 | 114 | ########################## 115 | ## Static code analysis ## 116 | ########################## 117 | 118 | roslint_cpp() 119 | -------------------------------------------------------------------------------- /rc_gazebo/config/actions.yaml: -------------------------------------------------------------------------------- 1 | actions: 2 | location_action: 3 | frame_id: location_action 4 | angle_error: 0.00004848 5 | pose_error: 0.006 6 | -------------------------------------------------------------------------------- /rc_gazebo/include/rc_gazebo/rc_action_hw_sim.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/17. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace rc_gazebo 11 | { 12 | struct ActionSimData 13 | { 14 | gazebo::physics::LinkPtr link_ptr; 15 | ros::Time time_stamp; 16 | rc_control::ActionData action_data; 17 | rc_control::ActionCmd action_cmd; 18 | }; 19 | 20 | class RcRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim 21 | { 22 | public: 23 | bool initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, 24 | const urdf::Model* urdf_model, 25 | std::vector transmissions) override; 26 | 27 | void readSim(ros::Time time, ros::Duration period) override; 28 | 29 | void writeSim(ros::Time time, ros::Duration period) override; 30 | 31 | private: 32 | void parseAction(XmlRpc::XmlRpcValue& action_sim_datas, const gazebo::physics::ModelPtr& parent_model); 33 | 34 | rc_control::ActionInterface rc_action_sim_interface_; 35 | gazebo::physics::WorldPtr world_; 36 | std::vector action_sim_datas_; 37 | 38 | double angle_error_; 39 | double cumulative_angle_error_; 40 | 41 | double pose_error_; 42 | double cumulative_poseX_error_; 43 | double cumulative_poseY_error_; 44 | double last_yaw_angle_; 45 | double last_poseX_; 46 | double last_poseY_; 47 | }; 48 | 49 | } // namespace rc_gazebo 50 | -------------------------------------------------------------------------------- /rc_gazebo/launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rc_gazebo/launch/field_rc21.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 34 | 35 | 36 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /rc_gazebo/launch/field_rc23.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 39 | 40 | -------------------------------------------------------------------------------- /rc_gazebo/launch/task_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 31 | 32 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /rc_gazebo/models/task_model/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | task_model 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rc_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rc_gazebo 4 | 0.0.0 5 | The rc_gazebo package 6 | 7 | 8 | 9 | 10 | myx 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | catkin 21 | 22 | roscpp 23 | roslint 24 | rc_common 25 | 26 | gazebo 27 | gazebo_ros 28 | gazebo_ros_control 29 | 30 | rc_description 31 | roboticsgroup_upatras_gazebo_plugins 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /rc_gazebo/rc_action_hw_sim_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | The action sensor simulation interface. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rc_gazebo/src/rc_action_hw_sim.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/17. 3 | // 4 | #include "rc_gazebo/rc_action_hw_sim.h" 5 | #include 6 | 7 | namespace rc_gazebo 8 | { 9 | bool RcRobotHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, 10 | gazebo::physics::ModelPtr parent_model, const urdf::Model* urdf_model, 11 | std::vector transmissions) 12 | { 13 | bool ret = DefaultRobotHWSim::initSim(robot_namespace, model_nh, parent_model, urdf_model, transmissions); 14 | gazebo_ros_control::DefaultRobotHWSim::registerInterface(&rc_action_sim_interface_); 15 | XmlRpc::XmlRpcValue xml_rpc_value; 16 | 17 | if (!model_nh.getParam("actions", xml_rpc_value)) 18 | ROS_WARN("No action specified"); 19 | else 20 | parseAction(xml_rpc_value, parent_model); 21 | world_ = parent_model->GetWorld(); // For gravity 22 | return ret; 23 | } 24 | 25 | void RcRobotHWSim::readSim(ros::Time time, ros::Duration period) 26 | { 27 | gazebo_ros_control::DefaultRobotHWSim::readSim(time, period); 28 | for (auto& action : action_sim_datas_) 29 | { 30 | std::mt19937 gen(time.toNSec()); 31 | ignition::math::Pose3d pose = action.link_ptr->WorldPose(); 32 | action.time_stamp = time; 33 | 34 | // std::uniform_real_distribution dist_angle_error(0.0, angle_error_ / period.toSec()); 35 | // cumulative_angle_error_ += dist_angle_error(gen); 36 | // 37 | // std::uniform_real_distribution dist_poseX_error(0.0, pose_error_ / (pose.X() - last_poseX_)); 38 | // cumulative_poseX_error_ += dist_poseX_error(gen); 39 | // 40 | // std::uniform_real_distribution dist_poseY_error(0.0, pose_error_ / (pose.Y() - last_poseY_)); 41 | // cumulative_poseY_error_ += dist_poseY_error(gen); 42 | 43 | action.action_data.yaw_angle = pose.Yaw() + cumulative_angle_error_; 44 | action.action_data.pitch_angle = pose.Pitch() + cumulative_angle_error_; 45 | action.action_data.roll_angle = pose.Roll() + cumulative_angle_error_; 46 | action.action_data.pose_x = pose.X() + cumulative_poseX_error_; 47 | action.action_data.pose_y = pose.Y() + cumulative_poseY_error_; 48 | action.action_data.yaw_acc = (pose.Yaw() - last_yaw_angle_) / period.toSec(); 49 | 50 | // ROS_INFO_STREAM("action.action_data.yaw_angle: " << action.action_data.yaw_angle); 51 | // ROS_INFO_STREAM("action.action_data.pitch_angle: " << action.action_data.pitch_angle); 52 | // ROS_INFO_STREAM("action.action_data.roll_angle: " << action.action_data.roll_angle); 53 | // ROS_INFO_STREAM("action.action_data.pose_x: " << action.action_data.pose_x); 54 | // ROS_INFO_STREAM("action.action_data.pose_y: " << action.action_data.pose_y); 55 | // ROS_INFO_STREAM("action.action_data.yaw_acc: " << action.action_data.yaw_acc); 56 | 57 | last_yaw_angle_ = pose.Yaw(); 58 | last_poseX_ = pose.X(); 59 | last_poseY_ = pose.Y(); 60 | } 61 | 62 | // Set cmd to zero to avoid crazy soft limit oscillation when not controller loaded 63 | for (auto& cmd : joint_effort_command_) 64 | cmd = 0; 65 | for (auto& cmd : joint_velocity_command_) 66 | cmd = 0; 67 | } 68 | 69 | void RcRobotHWSim::writeSim(ros::Time time, ros::Duration period) 70 | { 71 | gazebo_ros_control::DefaultRobotHWSim::writeSim(time, period); 72 | for (auto& action : action_sim_datas_) 73 | { 74 | ignition::math::Pose3d pose = action.link_ptr->WorldPose(); 75 | action.time_stamp = time; 76 | // calibration_state default id True. 77 | if (!action.action_cmd.calibration_state) 78 | { 79 | ROS_INFO_STREAM("Received calibration_cmd of action: " << action.action_data.name); 80 | action.action_cmd.calibration_state = true; 81 | } 82 | 83 | if (action.action_cmd.reset_state) 84 | { 85 | ROS_INFO_STREAM("Received reset_cmd of action: " << action.action_data.name); 86 | action.action_data.yaw_angle = 0.0; 87 | action.action_data.pitch_angle = 0.0; 88 | action.action_data.roll_angle = 0.0; 89 | action.action_data.pose_x = 0.0; 90 | action.action_data.pose_y = 0.0; 91 | action.action_data.yaw_acc = 0.0; 92 | 93 | action.action_cmd.reset_state = false; 94 | } 95 | 96 | if (action.action_cmd.update_yaw_state) 97 | { 98 | ROS_INFO_STREAM("Received update_yaw_cmd of action: " << action.action_data.name); 99 | action.action_data.yaw_angle = action.action_cmd.update_yaw; 100 | action.action_cmd.update_yaw_state = false; 101 | } 102 | 103 | if (action.action_cmd.update_x_state) 104 | { 105 | ROS_INFO_STREAM("Received update_x__cmd of action: " << action.action_data.name); 106 | action.action_data.pose_x = action.action_cmd.update_x; 107 | action.action_cmd.update_x_state = false; 108 | } 109 | 110 | if (action.action_cmd.update_y_state) 111 | { 112 | ROS_INFO_STREAM("Received update_y_cmd of action: " << action.action_data.name); 113 | action.action_data.pose_y = action.action_cmd.update_y; 114 | action.action_cmd.update_y_state = false; 115 | } 116 | } 117 | } 118 | 119 | void RcRobotHWSim::parseAction(XmlRpc::XmlRpcValue& action_sim_datas, const gazebo::physics::ModelPtr& parent_model) 120 | { 121 | ROS_ASSERT(action_sim_datas.getType() == XmlRpc::XmlRpcValue::TypeStruct); 122 | for (auto it = action_sim_datas.begin(); it != action_sim_datas.end(); ++it) 123 | { 124 | if (!it->second.hasMember("frame_id")) 125 | { 126 | ROS_ERROR_STREAM("Action " << it->first << " has no associated frame id."); 127 | continue; 128 | } 129 | else if (!it->second.hasMember("angle_error")) 130 | { 131 | ROS_ERROR_STREAM("Action " << it->first << " has no associated angle_error."); 132 | continue; 133 | } 134 | else if (!it->second.hasMember("pose_error")) 135 | { 136 | ROS_ERROR_STREAM("Action " << it->first << " has no associated pose_error."); 137 | continue; 138 | } 139 | XmlRpc::XmlRpcValue angle_error = action_sim_datas[it->first]["angle_error"]; 140 | ROS_ASSERT(angle_error.getType() == XmlRpc::XmlRpcValue::TypeDouble); 141 | angle_error_ = angle_error; 142 | XmlRpc::XmlRpcValue pose_error = action_sim_datas[it->first]["pose_error"]; 143 | ROS_ASSERT(pose_error.getType() == XmlRpc::XmlRpcValue::TypeDouble); 144 | pose_error_ = pose_error; 145 | 146 | cumulative_angle_error_ = 0.0; 147 | cumulative_poseX_error_ = 0.0; 148 | cumulative_poseY_error_ = 0.0; 149 | last_yaw_angle_ = 0.0; 150 | last_poseX_ = 0.0; 151 | last_poseY_ = 0.0; 152 | 153 | std::string frame_id = action_sim_datas[it->first]["frame_id"]; 154 | gazebo::physics::LinkPtr link_ptr = parent_model->GetLink(frame_id); 155 | if (link_ptr == nullptr) 156 | { 157 | ROS_WARN("Action %s is not specified in urdf.", it->first.c_str()); 158 | continue; 159 | } 160 | 161 | rc_control::ActionData action_data; 162 | action_data = { .name = it->first, 163 | .yaw_angle = 0.0, 164 | .pitch_angle = 0.0, 165 | .roll_angle = 0.0, 166 | .pose_x = 0.0, 167 | .pose_y = 0.0, 168 | .yaw_acc = 0.0 }; 169 | rc_control::ActionCmd action_cmd; 170 | action_cmd = { .calibration_state = true, 171 | .reset_state = false, 172 | .update_x_state = false, 173 | .update_x = 0.0, 174 | .update_y_state = false, 175 | .update_y = 0.0, 176 | .update_yaw_state = false, 177 | .update_yaw = 0.0 }; 178 | 179 | ActionSimData action_data_sim = { 180 | .link_ptr = link_ptr, .time_stamp = {}, .action_data = action_data, .action_cmd = action_cmd 181 | }; 182 | 183 | action_sim_datas_.push_back(action_data_sim); 184 | 185 | rc_control::ActionHandle action_sim_handle(it->first, &action_sim_datas_.back().action_data, 186 | &action_sim_datas_.back().action_cmd); 187 | rc_action_sim_interface_.registerHandle(action_sim_handle); 188 | } 189 | } 190 | 191 | } // namespace rc_gazebo 192 | 193 | PLUGINLIB_EXPORT_CLASS(rc_gazebo::RcRobotHWSim, gazebo_ros_control::RobotHWSim) 194 | GZ_REGISTER_MODEL_PLUGIN(gazebo_ros_control::GazeboRosControlPlugin) // Default plugin 195 | -------------------------------------------------------------------------------- /rc_gazebo/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 0.001 10 | 1000 11 | 12 | 13 | 14 | model://sun 15 | 16 | 17 | 18 | model://ground_plane 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /rc_gazebo/worlds/field_rc23.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 0 17 | 0 18 | 0 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 65535 34 | 35 | 36 | 37 | 38 | 100 39 | 50 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 10 48 | 49 | 50 | 0 51 | 52 | 53 | 0 0 1 54 | 100 100 55 | 56 | 57 | 58 | 62 | 63 | 64 | 0 65 | 0 66 | 0 67 | 68 | 69 | 0 0 -9.8 70 | 6e-06 2.3e-05 -4.2e-05 71 | 72 | 73 | 0.001 74 | 1 75 | 1000 76 | 77 | 78 | 0.4 0.4 0.4 1 79 | 0.7 0.7 0.7 1 80 | 1 81 | 82 | 83 | 84 | EARTH_WGS84 85 | 0 86 | 0 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 0 -0 0.136686 0 -0 0 94 | 13691.4 95 | 96 | 79068.1 97 | -222.476 98 | -0.000822824 99 | 77424.7 100 | -0.00872577 101 | 156194 102 | 103 | 104 | 105 | 0 0 0 0 -0 0 106 | 107 | 108 | 1 1 1 109 | /home/jialonglong/rc_ws/src/rc_software/two_layer/meshes/base_link.STL 110 | 111 | 112 | 10 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 0 0 0 0 -0 0 128 | 129 | 130 | 1 1 1 131 | /home/jialonglong/rc_ws/src/rc_software/two_layer/meshes/base_link.STL 132 | 133 | 134 | 135 | 0 136 | 0 137 | 0 138 | 139 | 0 0 0 0 -0 0 140 | 141 | 142 | 66 388000000 143 | 66 773807529 144 | 1684748279 911784906 145 | 66388 146 | 147 | 0 0 0 0 -0 0 148 | 1 1 1 149 | 150 | 0 0 0 0 -0 0 151 | 0 0 0 0 -0 0 152 | 0 0 0 0 -0 0 153 | 0 0 0 0 -0 0 154 | 155 | 156 | 157 | -0 0 -7.9e-05 -1.3e-05 0 0 158 | 1 1 1 159 | 160 | -0 0 -7.9e-05 -1.3e-05 0 0 161 | 0 0 0 0 -0 0 162 | -0.000126 1.38801 0 2.41746 -0.000904 1e-06 163 | -1.72723 19003.9 2.8e-05 0 -0 0 164 | 165 | 166 | 167 | 0 0 10 0 -0 0 168 | 169 | 170 | 171 | 172 | -16.2094 2.51131 14.8373 0 0.787643 -0.463805 173 | orbit 174 | perspective 175 | 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /rc_hw/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rc_hw) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | set(CMAKE_BUILD_TYPE "Debug") 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | roscpp 11 | roslint 12 | 13 | rc_common 14 | rc_msgs 15 | hardware_interface 16 | controller_interface 17 | transmission_interface 18 | joint_limits_interface 19 | controller_manager 20 | urdf 21 | realtime_tools 22 | serial 23 | ) 24 | 25 | ## Find system libraries 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 | LIBRARIES 41 | ${PROJECT_NAME}_loader_plugins 42 | CATKIN_DEPENDS 43 | roscpp 44 | roslint 45 | 46 | rc_common 47 | rc_msgs 48 | hardware_interface 49 | controller_interface 50 | transmission_interface 51 | joint_limits_interface 52 | controller_manager 53 | urdf 54 | realtime_tools 55 | serial 56 | DEPENDS 57 | ) 58 | 59 | ########### 60 | ## Build ## 61 | ########### 62 | 63 | ## Specify additional locations of header files 64 | include_directories( 65 | include 66 | ${catkin_INCLUDE_DIRS} 67 | ) 68 | 69 | ## Declare cpp executables 70 | add_executable(${PROJECT_NAME} 71 | src/${PROJECT_NAME}.cpp 72 | src/hardware_interface/hardware_interface.cpp 73 | src/hardware_interface/parse.cpp 74 | src/hardware_interface/socketcan.cpp 75 | src/hardware_interface/can_bus.cpp 76 | src/hardware_interface/gpio_manager.cpp 77 | src/hardware_interface/action_manager.cpp 78 | src/control_loop.cpp 79 | src/hardware_interface/DT35_laser.cpp) 80 | 81 | ## Specify libraries to link executable targets against 82 | target_link_libraries(${PROJECT_NAME} 83 | ${catkin_LIBRARIES} 84 | ) 85 | 86 | ############# 87 | ## Install ## 88 | ############# 89 | 90 | # Mark executables and/or libraries for installation 91 | install(TARGETS ${PROJECT_NAME} 92 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | ) 96 | 97 | # Mark cpp header files for installation 98 | install(DIRECTORY include/${PROJECT_NAME}/ 99 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 100 | FILES_MATCHING PATTERN "*.h" 101 | ) 102 | 103 | # Mark other files for installation 104 | install(DIRECTORY config launch 105 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 106 | ) 107 | #install( 108 | # FILES rm_hw_loader_plugins.xml 109 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 110 | #) 111 | 112 | 113 | ############# 114 | ## Testing ## 115 | ############# 116 | 117 | ## Add gtest based cpp test target and link libraries 118 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rc_hw.cpp) 119 | # if(TARGET ${PROJECT_NAME}-test) 120 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 121 | # endif() 122 | 123 | ## Add folders to be run by python nosetests 124 | # catkin_add_nosetests(test) 125 | 126 | roslint_cpp() 127 | -------------------------------------------------------------------------------- /rc_hw/config/actuator_coefficient.yaml: -------------------------------------------------------------------------------- 1 | actuator_coefficient: 2 | rm_3508: # RoboMaster 3508 without reducer 3 | act2pos: 0.0007669903 # 2PI/8192 4 | act2vel: 0.1047197551 # 2PI/60 5 | act2effort: 1.90702994e-5 # 20/16384*0.0156223893 6 | effort2act: 52437.561519 # 1/act2effort 7 | max_out: 16384 8 | rm_6020: # RoboMaster 3508 motor 9 | act2pos: 0.0007670840 # 2PI/8192 10 | act2vel: 0.1047197551 # 2PI/60 11 | act2effort: 5.880969e-5 # special coefficient identify by hands... 12 | effort2act: 25000 # 13 | max_out: 30000 14 | rm_2006: # RoboMaster 2006 motor 15 | act2pos: 2.13078897e-5 # 2PI/8192*(1/36) 16 | act2vel: 0.0029088820 # 2PI/60*(1/36) 17 | act2effort: 0.00018 #10/10000*0.18 18 | effort2act: 5555.5555555 # 1/act2effort 19 | max_out: 10000 20 | cheetah: # MIT cheetah motor 21 | act2pos: 3.81475547e-4 # 25/65535 22 | act2vel: 0.0317446031 # 130/4095 23 | act2effort: 0.008791208 # 36/4095 24 | pos2act: 2621.4 # 65535/25 25 | vel2act: 31.5 # 4095/130 26 | effort2act: 113.75 # 4095/36 27 | max_out: 0 28 | act2pos_offset: -12.5 29 | act2vel_offset: -65.0 30 | act2effort_offset: -18.0 31 | kp2act: 8.19 # 4095/500 32 | kd2act: 819 # 4095/5 33 | -------------------------------------------------------------------------------- /rc_hw/config/hw_config_template.yaml: -------------------------------------------------------------------------------- 1 | rc_hw: 2 | bus: 3 | - can0 4 | - can1 5 | loop_frequency: 1000 6 | cycle_time_error_threshold: 0.001 7 | # Configurations of the actuators 8 | actuators: 9 | right_front_wheel_motor: 10 | bus: can0 11 | id: 0x201 12 | type: rm_3508 13 | lp_cutoff_frequency: 150 14 | # need calibration: true #set to true when this actuator need to be calibrated and it defaults to false when it is not been set. 15 | imus: 16 | gimbal_imu: 17 | frame_id: gimbal_imu 18 | bus: can1 19 | id: 0x300 20 | orientation_covariance_diagonal: [ 0.0012, 0.0012, 0.0012 ] 21 | angular_velocity_covariance: [ 0.0004, 0.0004, 0.0004 ] 22 | linear_acceleration_covariance: [ 0.01, 0.01, 0.01 ] 23 | angular_vel_coeff: 0.0010652644 24 | accel_coeff: 0.0017944335 25 | temp_coeff: 0.125 26 | temp_offset: 23.0 27 | filter: complementary 28 | do_bias_estimation: false 29 | do_adaptive_gain: true 30 | gain_acc: 0.0003 31 | gpios: 32 | front_lamp: 33 | pin: 245 34 | direction: out 35 | back_lamp: 36 | pin: 246 37 | direction: out 38 | 39 | actions: 40 | location_action: # action names 41 | serial_port: "/dev/usbDbus" 42 | test_action: 43 | serial_port: "/dev/ttyUSB0" 44 | 45 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/control_loop.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include "rc_hw/hardware_interface/hardware_interface.h" 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace rc_hw 17 | { 18 | using namespace std::chrono; 19 | using clock = high_resolution_clock; 20 | 21 | class RcRobotHWLoop 22 | { 23 | public: 24 | /** \brief Create controller manager. Load loop frequency. Start control loop which call @ref 25 | * rc_hw::RcRobotHWLoop::update() in a frequency. 26 | * 27 | * @param nh Node-handle of a ROS node. 28 | * @param hardware_interface A pointer which point to hardware_interface. 29 | */ 30 | RcRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr hardware_interface); 31 | 32 | ~RcRobotHWLoop(); 33 | /** \brief Timed method that reads current hardware's state, runs the controller code once and sends the new commands 34 | * to the hardware. 35 | * 36 | * Timed method that reads current hardware's state, runs the controller code once and sends the new commands to the 37 | * hardware. 38 | * 39 | * Note: we do not use the TimerEvent time difference because it does NOT guarantee that the time source is strictly 40 | * linearly increasing. 41 | */ 42 | 43 | void update(); 44 | 45 | private: 46 | // Startup and shutdown of the internal node inside a roscpp program 47 | ros::NodeHandle nh_; 48 | 49 | // Settings 50 | double cycle_time_error_threshold_{}; 51 | 52 | // Timing 53 | std::thread loop_thread_; 54 | std::atomic_bool loop_running_; 55 | double loop_hz_{}; 56 | ros::Duration elapsed_time_; 57 | clock::time_point last_time_; 58 | 59 | /** ROS Controller Manager and Runner 60 | 61 | This class advertises a ROS interface for loading, unloading, starting, and 62 | stopping ros_control-based controllers. It also serializes execution of all 63 | running controllers in \ref update. 64 | **/ 65 | std::shared_ptr controller_manager_; 66 | 67 | // Abstract Hardware Interface for your robot 68 | std::shared_ptr hardware_interface_; 69 | }; 70 | } // namespace rc_hw -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/DT35_laser.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jialonglong on 23-7-13. 3 | // 4 | 5 | #pragma once 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "action_manager.h" 14 | #include 15 | #include 16 | namespace rc_hw 17 | { 18 | class DT35_laser 19 | { 20 | public: 21 | bool init(std::string serialPort); 22 | void readDT35_laser(const ros::Time &time, const ros::Duration &period); 23 | 24 | std::vector dt35_data_values; 25 | protected: 26 | private: 27 | void unPack(const uint8_t data); 28 | struct dt35_laser 29 | { 30 | int data; 31 | }dt35Laser1,dt35Laser2,dt35Laser3,dt35Laser4; 32 | union receiveData 33 | { 34 | int d; 35 | unsigned char data[4]; 36 | }laserData1,laserData2,laserData3,laserData4; 37 | std::vector serial_flam_marks_; 38 | std::vector serial_fds_{}; 39 | int fd; 40 | RampFilter*laser1_{}, *laser2_{}, *laser3_{},*laser4_{}; 41 | }; 42 | }//namespace rc_hw 43 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/action_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace rc_hw 12 | { 13 | union F1DataTransCh4 14 | { 15 | float f_data; 16 | char ch_data[4]; 17 | }; 18 | union F6DataTransCh24 19 | { 20 | char ch_data[24]; 21 | float f_data4[6]; 22 | }; 23 | enum SerialFlamMark 24 | { 25 | START, 26 | FIRST_HEARD, 27 | SECOND_HEARD, 28 | DATA, 29 | FIRST_TAIL 30 | }; 31 | 32 | struct Action 33 | { 34 | serial::Serial serial_value; 35 | }; 36 | class ActionManager 37 | { 38 | public: 39 | explicit ActionManager(); 40 | ~ActionManager(); 41 | 42 | bool initAction(std::string serialPort, std::shared_ptr serial1); 43 | void readAction(const ros::Time& time, const ros::Duration& period); 44 | void writeAction(const ros::Time& time, const ros::Duration& period); 45 | 46 | std::vector actions; 47 | 48 | std::vector action_data_values; 49 | std::vector action_command_values; 50 | 51 | private: 52 | void unPack(const int id, const uint8_t data); 53 | bool writCmd(const int id, const std::string cmd); 54 | bool writCmd(const int id, const std::string cmd, const float data); 55 | 56 | std::vector serial_fds_{}; 57 | std::vector cmd_trans_; 58 | std::vector data_trans_; 59 | std::vector last_write_times_; 60 | std::vector serial_flam_marks_; 61 | std::vector serial_date_poses_; 62 | }; 63 | } // namespace rc_hw 64 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/can_bus.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include "rc_hw/hardware_interface/socketcant.h" 9 | #include "rc_hw/hardware_interface/types.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace rc_hw 16 | { 17 | struct CanFrameStamp 18 | { 19 | can_frame frame; 20 | ros::Time stamp; 21 | }; 22 | 23 | class CanBus 24 | { 25 | public: 26 | /** \brief 27 | * Initialize device at can_device, retry if fail. Set up header of CAN 28 | frame. 29 | * 30 | * \param bus_name Bus's name(example: can0). 31 | * \param data_ptr Pointer which point to CAN data. 32 | */ 33 | CanBus(const std::string& bus_name, CanDataPtr data_ptr, int thread_priority); 34 | /** \brief Read active data from read_buffer_ to data_ptr_, such as 35 | position, 36 | * velocity, torque and so on. Clear read_buffer_ after reading. 37 | * 38 | * \param time ROS time, but it doesn't be used. 39 | */ 40 | void read(ros::Time time); 41 | /** \brief Write commands to can bus. 42 | * 43 | */ 44 | void write(); 45 | 46 | void write(can_frame* frame); 47 | 48 | const std::string bus_name_; 49 | 50 | private: 51 | /** \brief This function will be called when CAN bus receive message. It 52 | push 53 | * frame which received into a vector: read_buffer_. 54 | * 55 | * @param frame The frame which socketcan receive. 56 | */ 57 | void frameCallback(const can_frame& frame); 58 | 59 | can::SocketCAN socket_can_; 60 | CanDataPtr data_ptr_; 61 | std::vector read_buffer_; 62 | 63 | can_frame rm_frame0_{}; // for id 0x201~0x204 64 | can_frame rm_frame1_{}; // for id 0x205~0x208 65 | 66 | mutable std::mutex mutex_; 67 | }; 68 | 69 | class DM_Cheetah{ 70 | public: 71 | static void DM_Cheetah_control_cmd(can_frame &frame, uint8_t cmd); 72 | private: 73 | can::SocketCAN socket_can_; 74 | }; 75 | 76 | } // namespace rc_hw 77 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/gpio_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace rc_hw 17 | { 18 | class GpioManager 19 | { 20 | public: 21 | explicit GpioManager(); 22 | ~GpioManager(); 23 | 24 | void setGpioDirection(rc_control::GpioData gpioData); 25 | void readGpio(); 26 | void writeGpio(); 27 | 28 | std::vector gpio_state_values; 29 | std::vector gpio_command_values; 30 | }; 31 | } // namespace rc_hw 32 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/hardware_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // ROS 14 | #include 15 | #include 16 | #include 17 | 18 | // ROS control 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "can_bus.h" 34 | #include "gpio_manager.h" 35 | #include "action_manager.h" 36 | #include "DT35_laser.h" 37 | 38 | namespace rc_hw 39 | { 40 | class RcRobotHW : public hardware_interface::RobotHW 41 | { 42 | public: 43 | RcRobotHW() = default; 44 | /** \brief Get necessary params from param server. Init hardware_interface. 45 | * 46 | * Get params from param server and check whether these params are set. Load 47 | * urdf of robot. Set up transmission and joint limit. Get configuration of 48 | * can bus and create data pointer which point to data received from Can 49 | bus. 50 | * 51 | * @param root_nh Root node-handle of a ROS node. 52 | * @param robot_hw_nh Node-handle for robot hardware. 53 | * @return True when init successful, False when failed. 54 | */ 55 | bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override; 56 | /** \brief Comunicate with hardware. Get datas, status of robot. 57 | * 58 | * Call @ref rc_hw::CanBus::read(). Check whether temperature of actuator is 59 | * too high and whether actuator is offline. Propagate actuator state to 60 | joint 61 | * state for the stored transmission. Set all cmd to zero to avoid crazy 62 | soft 63 | * limit oscillation when not controller loaded(all controllers update after 64 | * read()). 65 | * 66 | * @param time Current time 67 | * @param period Current time - last time 68 | */ 69 | void read(const ros::Time& time, const ros::Duration& period) override; 70 | 71 | /** \brief Comunicate with hardware. Publish command to robot. 72 | * 73 | * Propagate joint state to actuator state for the stored 74 | * transmission. Limit cmd_effort into suitable value. Call @ref 75 | * rc_hw::CanBus::write(). Publish actuator current state. 76 | * 77 | * @param time Current time 78 | * @param period Current time - last time 79 | */ 80 | void write(const ros::Time& time, const ros::Duration& period) override; 81 | 82 | void setCanBusThreadPriority(int thread_priority); 83 | 84 | private: 85 | /** \brief Check whether some coefficients that are related to actuator are 86 | * set up and load these coefficients. 87 | * 88 | * Check whether some coefficients that are related to actuator are set up 89 | and 90 | * load these coefficients. 91 | * 92 | * @param act_coeffs Coefficients you want to check and load. 93 | * @return True if all coefficients are set up. 94 | */ 95 | bool parseActCoeffs(XmlRpc::XmlRpcValue& act_coeffs); 96 | 97 | /** \brief Check whether actuator is specified and load specified params. 98 | * 99 | * Check whether actuator is specified and load specified params. 100 | * 101 | * @param act_datas Params you want to check and load. 102 | * @param robot_hw_nh Root node-handle of a ROS node. 103 | * @return True if all params are set up. 104 | */ 105 | bool parseActData(XmlRpc::XmlRpcValue& act_datas, ros::NodeHandle& robot_hw_nh); 106 | 107 | /** \brief Check whether some params that are related to imu are set up and 108 | * load these params. 109 | * 110 | * Check whether some params that are related to imu are set up and load 111 | these 112 | * params. 113 | * 114 | * @param imu_datas Params you want to check 115 | * @param robot_hw_nh Root node-handle of a ROS node 116 | * @return True if all params are set up. 117 | */ 118 | bool parseImuData(XmlRpc::XmlRpcValue& imu_datas, ros::NodeHandle& robot_hw_nh); 119 | 120 | /** \brief Check whether somme params that are related to gpio are set up and 121 | * load these params. 122 | * 123 | * Check whether somme params that are related to gpio are set up and load 124 | * these params. 125 | * 126 | * @param gpio_datas Params you want to check and load 127 | * @param robot_hw_nh A handle of a ROS node 128 | * @return True if all params are set up. 129 | */ 130 | bool parseGpioData(XmlRpc::XmlRpcValue& gpio_datas, ros::NodeHandle& robot_hw_nh); 131 | 132 | /** \brief Check whether somme params that are related to action are set up and 133 | * load these params. 134 | * 135 | * Check whether somme params that are related to action are set up and load 136 | * these params. 137 | * 138 | * @param action_datas Params you want to check and load 139 | * @param robot_hw_nh A handle of a ROS node 140 | * @return True if all params are set up. 141 | */ 142 | bool parseActionData(XmlRpc::XmlRpcValue& action_datas, ros::NodeHandle& robot_hw_nh); 143 | 144 | /** \brief Check whether somme params that are related to dt35 are set up and 145 | * load these params. 146 | * 147 | * Check whether somme params that are related to dt35 are set up and load 148 | * these params. 149 | * 150 | * @param dt35_datas Params you want to check and load 151 | * @param robot_hw_nh A handle of a ROS node 152 | * @return True if all params are set up. 153 | */ 154 | bool parseDT35Data(XmlRpc::XmlRpcValue& dt35_datas, ros::NodeHandle& robot_hw_nh); 155 | /** \brief Load urdf of robot from param server. 156 | * 157 | * Load urdf of robot from param server. 158 | * 159 | * @param root_nh Root node-handle of a ROS node 160 | * @return True if successful. 161 | */ 162 | bool loadUrdf(ros::NodeHandle& root_nh); 163 | 164 | bool setupTransmission(ros::NodeHandle& root_nh); 165 | /** \brief Set up joint limit. 166 | * 167 | * Set up joint limit. 168 | * 169 | * @param root_nh Root node-handle of a ROS node. 170 | * @return True if successful. 171 | */ 172 | bool setupJointLimit(ros::NodeHandle& root_nh); 173 | /** \brief Publish actuator's state to a topic named "/actuator_states". 174 | * 175 | * Publish actuator's state to a topic named "/actuator_states". 176 | * 177 | * @param time Current time 178 | */ 179 | void publishActuatorState(const ros::Time& time); 180 | 181 | bool is_actuator_specified_ = false; 182 | int thread_priority_; 183 | // Interface 184 | std::vector can_buses_{}; 185 | GpioManager gpio_manager_{}; 186 | ActionManager action_manager_{}; 187 | DT35_laser dt35_laser_{}; 188 | 189 | rc_control::ActionInterface action_interface_; 190 | rc_control::SharpIRInterface sharp_ir_interface_; 191 | rc_control::GpioStateInterface gpio_state_interface_; 192 | rc_control::GpioCommandInterface gpio_command_interface_; 193 | hardware_interface::ActuatorStateInterface act_state_interface_; 194 | rc_control::ActuatorExtraInterface act_extra_interface_; 195 | hardware_interface::EffortActuatorInterface effort_act_interface_; 196 | rc_control::RobotStateInterface robot_state_interface_; 197 | hardware_interface::ImuSensorInterface imu_sensor_interface_; 198 | rc_control::RcImuSensorInterface rc_imu_sensor_interface_; 199 | std::unique_ptr transmission_loader_{}; 200 | transmission_interface::RobotTransmissions robot_transmissions_; 201 | transmission_interface::ActuatorToJointStateInterface* act_to_jnt_state_{}; 202 | transmission_interface::JointToActuatorEffortInterface* jnt_to_act_effort_{}; 203 | joint_limits_interface::EffortJointSaturationInterface effort_jnt_saturation_interface_; 204 | joint_limits_interface::EffortJointSoftLimitsInterface effort_jnt_soft_limits_interface_; 205 | std::vector effort_joint_handles_{}; 206 | 207 | // URDF model of the robot 208 | std::string urdf_string_; // for transmission 209 | std::shared_ptr urdf_model_; // for limit 210 | 211 | // Actuator 212 | std::unordered_map type2act_coeffs_{}; 213 | std::unordered_map> bus_id2act_data_{}; 214 | 215 | // Imu 216 | std::unordered_map> bus_id2imu_data_{}; 217 | 218 | ros::Time last_publish_time_; 219 | std::shared_ptr> actuator_state_pub_; 220 | }; 221 | 222 | } // namespace rc_hw 223 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/socketcant.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | // Multi-threading 11 | #include 12 | #include 13 | 14 | namespace can 15 | { 16 | class SocketCAN 17 | { 18 | private: 19 | ifreq interface_request_{}; 20 | sockaddr_can address_{}; 21 | pthread_t receiver_thread_id_{}; 22 | 23 | public: 24 | /** 25 | * CAN socket file descriptor 26 | */ 27 | int sock_fd_ = -1; 28 | /** 29 | * Request for the child thread to terminate 30 | */ 31 | bool terminate_receiver_thread_ = false; 32 | bool receiver_thread_running_ = false; 33 | 34 | SocketCAN() = default; 35 | ~SocketCAN(); 36 | 37 | /** \brief Open and bind socket. 38 | * 39 | * \param interface bus's name(example: can0). 40 | * \param handler Pointer to a function which shall be called when frames 41 | are 42 | * being received from the CAN bus. 43 | * 44 | * \returns \c true if it successfully open and bind socket. 45 | */ 46 | bool open(const std::string& interface, boost::function handler, int thread_priority); 47 | /** \brief Close and unbind socket. 48 | * 49 | */ 50 | void close(); 51 | /** \brief Returns whether the socket is open or closed. 52 | * 53 | * \returns \c True if socket has opened. 54 | */ 55 | bool isOpen() const; 56 | /** \brief Sends the referenced frame to the bus. 57 | * 58 | * \param frame referenced frame which you want to send. 59 | */ 60 | void write(can_frame* frame) const; 61 | /** \brief Starts a new thread, that will wait for socket events. 62 | * 63 | */ 64 | bool startReceiverThread(int thread_priority); 65 | /** 66 | * Pointer to a function which shall be called 67 | * when frames are being received from the CAN bus 68 | */ 69 | boost::function reception_handler; 70 | }; 71 | 72 | } // namespace can 73 | -------------------------------------------------------------------------------- /rc_hw/include/rc_hw/hardware_interface/types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/11. 3 | // 4 | // ref:https://github.com/rm-controls 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace rc_hw 14 | { 15 | struct ActCoeff 16 | { 17 | double act2pos, act2vel, act2effort, pos2act, vel2act, effort2act, max_out, act2pos_offset, act2vel_offset, 18 | act2effort_offset, kp2act, 19 | kd2act; // for MIT Cheetah motor 20 | }; 21 | 22 | struct ActData 23 | { 24 | std::string name; 25 | std::string type; 26 | ros::Time stamp; 27 | uint64_t seq; 28 | bool halted = false, need_calibration = false, calibrated = false, calibration_reading = false; 29 | uint16_t q_raw; 30 | int16_t qd_raw; 31 | uint8_t temp; 32 | int64_t q_circle; 33 | uint16_t q_last; 34 | double frequency; 35 | double pos, vel, effort; 36 | double cmd_pos, cmd_vel, cmd_effort, exe_effort; 37 | double offset; 38 | // For multiple cycle under absolute encoder (RoboMaster motor) 39 | LowPassFilter* lp_filter; 40 | }; 41 | 42 | struct ImuData 43 | { 44 | ros::Time time_stamp; 45 | std::string imu_name; 46 | double ori[4]; 47 | double angular_vel[3], linear_acc[3]; 48 | double angular_vel_offset[3]; 49 | double ori_cov[9], angular_vel_cov[9], linear_acc_cov[9]; 50 | double temperature, angular_vel_coeff, accel_coeff, temp_coeff, temp_offset; 51 | bool accel_updated, gyro_updated, camera_trigger; 52 | bool enabled_trigger; 53 | rc_common::ImuFilterBase* imu_filter; 54 | }; 55 | 56 | struct TofData 57 | { 58 | double strength; 59 | double distance; 60 | }; 61 | 62 | struct CanDataPtr 63 | { 64 | std::unordered_map* type2act_coeffs_; 65 | std::unordered_map* id2act_data_; 66 | std::unordered_map* id2imu_data_; 67 | std::unordered_map* id2tof_data_; 68 | }; 69 | 70 | struct ActionData 71 | { 72 | ros::Time time_stamp; 73 | double yaw_angle, pitch_angle, roll_angle, pose_x, pose_y, yaw_acc; 74 | }; 75 | } // namespace rc_hw 76 | -------------------------------------------------------------------------------- /rc_hw/launch/rc_hw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | -------------------------------------------------------------------------------- /rc_hw/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rc_hw 4 | 0.0.0 5 | The rc_hw package 6 | 7 | 8 | 9 | 10 | myx 11 | 12 | 13 | 14 | 15 | BSD 16 | 17 | 18 | catkin 19 | 20 | 21 | roscpp 22 | roslint 23 | 24 | rc_common 25 | rc_msgs 26 | hardware_interface 27 | controller_interface 28 | transmission_interface 29 | joint_limits_interface 30 | controller_manager 31 | urdf 32 | realtime_tools 33 | serial 34 | 35 | 36 | -------------------------------------------------------------------------------- /rc_hw/rc_hw_loader_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rc_hw/src/control_loop.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 12/30/20. 36 | // 37 | #include "rc_hw/control_loop.h" 38 | 39 | namespace rc_hw 40 | { 41 | RcRobotHWLoop::RcRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr hardware_interface) 42 | : nh_(nh), hardware_interface_(std::move(hardware_interface)) 43 | { 44 | // Load ros params 45 | int error = 0, thread_priority; 46 | ros::NodeHandle nh_p("~"); 47 | error += !nh_p.getParam("loop_frequency", loop_hz_); 48 | error += !nh_p.getParam("cycle_time_error_threshold", cycle_time_error_threshold_); 49 | error += !nh_p.getParam("thread_priority", thread_priority); 50 | if (error > 0) 51 | { 52 | char error_message[] = "could not retrieve one of the required parameters\n\trc_hw/loop_hz or " 53 | "rc_hw/cycle_time_error_threshold or " 54 | "rc_hw/thread_priority"; 55 | ROS_ERROR_STREAM(error_message); 56 | throw std::runtime_error(error_message); 57 | } 58 | 59 | // Initialise the hardware interface: 60 | // 1. retrieve configuration from rosparam 61 | // 2. initialise the hardware and interface it with ros_control 62 | hardware_interface_->setCanBusThreadPriority(thread_priority); 63 | hardware_interface_->init(nh, nh_p); 64 | // Create the controller manager 65 | controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_)); 66 | 67 | // Get current time for use with first update 68 | last_time_ = clock::now(); 69 | 70 | // Setup loop thread 71 | loop_thread_ = std::thread([&]() { 72 | while (loop_running_) 73 | { 74 | if (loop_running_) 75 | update(); 76 | } 77 | }); 78 | sched_param sched{ .sched_priority = thread_priority }; 79 | if (pthread_setschedparam(loop_thread_.native_handle(), SCHED_FIFO, &sched) != 0) 80 | ROS_WARN("Failed to set threads priority (one possible reason could be that the user and the group permissions " 81 | "are not set properly.).\n"); 82 | } 83 | 84 | void RcRobotHWLoop::update() 85 | { 86 | const auto current_time = clock::now(); 87 | // Compute desired duration rounded to clock decimation 88 | const duration desired_duration(1.0 / loop_hz_); 89 | // Get change in time 90 | duration time_span = duration_cast>(current_time - last_time_); 91 | elapsed_time_ = ros::Duration(time_span.count()); 92 | last_time_ = current_time; 93 | 94 | // Check cycle time for excess delay 95 | const double cycle_time_error = (elapsed_time_ - ros::Duration(desired_duration.count())).toSec(); 96 | if (cycle_time_error > cycle_time_error_threshold_) 97 | ROS_WARN_STREAM("Cycle time exceeded error threshold by: " << cycle_time_error - cycle_time_error_threshold_ 98 | << "s, " 99 | << "cycle time: " << elapsed_time_ << "s, " 100 | << "threshold: " << cycle_time_error_threshold_ << "s"); 101 | // Input 102 | // get the hardware's state 103 | hardware_interface_->read(ros::Time::now(), elapsed_time_); 104 | 105 | // Control 106 | // let the controller compute the new command (via the controller manager) 107 | controller_manager_->update(ros::Time::now(), elapsed_time_); 108 | 109 | // Output 110 | // send the new command to hardware 111 | hardware_interface_->write(ros::Time::now(), elapsed_time_); 112 | 113 | // Sleep 114 | const auto sleep_till = current_time + duration_cast(desired_duration); 115 | std::this_thread::sleep_until(sleep_till); 116 | } 117 | 118 | RcRobotHWLoop::~RcRobotHWLoop() 119 | { 120 | loop_running_ = false; 121 | if (loop_thread_.joinable()) 122 | loop_thread_.join(); 123 | } 124 | } // namespace rc_hw 125 | -------------------------------------------------------------------------------- /rc_hw/src/hardware_interface/DT35_laser.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jialonglong on 23-7-13. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | namespace rc_hw 9 | { 10 | bool DT35_laser::init(std::string serialPort) 11 | { 12 | char* port = (char*)serialPort.data(); 13 | fd = open(port,O_RDWR | O_NOCTTY | O_SYNC); 14 | if(fd < 0) 15 | { 16 | ROS_ERROR_STREAM("Unable to open serial of DT35. Serial port:" << serialPort); 17 | return false; 18 | } 19 | struct termios tty; 20 | if (tcgetattr(fd, &tty) != 0) 21 | { 22 | ROS_ERROR_STREAM("Unable to get serial port attributes"); 23 | return false; 24 | } 25 | //To set some basic data of serial 26 | cfsetospeed(&tty,B115200);//bitrate 27 | cfsetispeed(&tty,B115200); 28 | tty.c_cflag &= ~PARENB; 29 | tty.c_cflag &= ~CSTOPB; 30 | tty.c_cflag &= ~CSIZE; 31 | tty.c_cflag |= CS8; 32 | tty.c_cflag &= ~CRTSCTS; 33 | if (tcsetattr(fd,TCSANOW,&tty) !=0) 34 | { 35 | ROS_ERROR_STREAM("Unable to get serial port attributes"); 36 | return false; 37 | } 38 | serial_fds_.push_back(fd); 39 | ROS_INFO_STREAM("Successful to open " << serialPort << " port."); 40 | return true; 41 | } 42 | 43 | void DT35_laser::readDT35_laser(const ros::Time &time, const ros::Duration &period) 44 | { 45 | uint8_t buffer[1]; 46 | int timeout = 0; // time out of one package 47 | while (timeout < 5) 48 | { 49 | // Read a byte with a timeout of 100 ms // 50 | fd_set set; 51 | struct timeval timeout_tv; 52 | FD_ZERO(&set); 53 | FD_SET(fd, &set); 54 | timeout_tv.tv_sec = 0; 55 | timeout_tv.tv_usec = 100000; // 100 ms 56 | int select_ret = select(fd + 1, &set, NULL, NULL, &timeout_tv); 57 | if (select_ret == -1) 58 | { 59 | // Error in select() 60 | ROS_ERROR_STREAM("Error in select() function"); 61 | return; 62 | } 63 | else if (select_ret == 0) 64 | { 65 | // Timeout 66 | timeout++; 67 | } 68 | else 69 | { 70 | // Data is available to read 71 | size_t n = ::read(fd, buffer, 1); 72 | if (n == 1) 73 | { 74 | unPack(buffer[0]); 75 | } 76 | } 77 | } 78 | } 79 | 80 | 81 | void DT35_laser::unPack(const uint8_t data) 82 | { 83 | static unsigned char USARTBufferIndex= 0; 84 | static short j=0,k=0; 85 | static short dataLength=0; 86 | static unsigned char buffer[22]={0}; 87 | const int buffer_size=22; 88 | unsigned char header[2]={0x55,0xaa}; 89 | const unsigned char ender[2]={0x0d,0x0a}; 90 | static bool start= true; 91 | //check header 92 | if (start) 93 | { 94 | if (data == 0x55) 95 | { 96 | buffer[0]=header[0]; 97 | } 98 | if (data == 0xaa) 99 | { 100 | buffer[1]=header[1]; 101 | USARTBufferIndex=0; 102 | start = false; 103 | } 104 | else 105 | { 106 | buffer[0]=buffer[1]; 107 | } 108 | } 109 | else 110 | { 111 | switch(USARTBufferIndex) 112 | { 113 | //check the length 114 | case 0: 115 | buffer[2]=data; 116 | dataLength = buffer[2]; 117 | USARTBufferIndex++; 118 | break; 119 | //check all data 120 | case 1: 121 | buffer[j+3]=data; 122 | j++; 123 | if (j>=dataLength-1) 124 | { 125 | j=0; 126 | USARTBufferIndex++; 127 | } 128 | break; 129 | case 2: 130 | buffer[2+dataLength]=data; 131 | USARTBufferIndex++; 132 | break; 133 | case 3: 134 | if(k==0) 135 | { 136 | k++; 137 | } 138 | else if (k==1) 139 | { 140 | for (k=0;k<4;k++) 141 | { 142 | if (k + 3 < buffer_size) 143 | DT35_laser::laserData1.data[k] = buffer[k + 3]; // laser_1 144 | else 145 | ROS_WARN("laser_1 stack smashing detected,please check your settings"); 146 | if (k + 7 < buffer_size) 147 | DT35_laser::laserData2.data[k] = buffer[k + 7]; // laser_2 148 | else 149 | ROS_WARN("laser_2 stack smashing detected,please check your settings"); 150 | if (k + 11 < buffer_size) 151 | DT35_laser::laserData3.data[k] = buffer[k + 11]; // laser_3 152 | else 153 | ROS_WARN("laser_3 stack smashing detected,please check your settings"); 154 | if (k + 15 < buffer_size) 155 | DT35_laser::laserData4.data[k] = buffer[k + 15]; // laser_4 156 | else 157 | ROS_WARN("laser_4 stack smashing detected,please check your settings"); 158 | } 159 | laser1_->input(DT35_laser::laserData1.d); 160 | laser2_->input(DT35_laser::laserData2.d); 161 | laser3_->input(DT35_laser::laserData3.d); 162 | laser4_->input(DT35_laser::laserData4.d); 163 | DT35_laser::dt35Laser1.data=laser1_->output(); 164 | DT35_laser::dt35Laser2.data=laser2_->output(); 165 | DT35_laser::dt35Laser3.data=laser3_->output(); 166 | DT35_laser::dt35Laser4.data=laser4_->output(); 167 | USARTBufferIndex = 0 ; 168 | dataLength = 0 ; 169 | j = 0 ; 170 | k = 0 ; 171 | start =true ; 172 | } 173 | break; 174 | default: 175 | break; 176 | } 177 | } 178 | } 179 | 180 | }//namespace rc_hw -------------------------------------------------------------------------------- /rc_hw/src/hardware_interface/action_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/12. 3 | // 4 | #include 5 | #include 6 | 7 | #include /* File control definitions */ 8 | #include 9 | #include 10 | 11 | namespace rc_hw 12 | { 13 | ActionManager::ActionManager() 14 | { 15 | } 16 | 17 | ActionManager::~ActionManager() 18 | { 19 | } 20 | 21 | bool ActionManager::initAction(std::string serialPort, std::shared_ptr serial1) 22 | { 23 | char* port = (char*)serialPort.data(); 24 | int fd = open(port, O_RDWR | O_NOCTTY); 25 | if (fd == -1) 26 | { 27 | ROS_ERROR_STREAM("Unable to open serial of action. Serial port:" << serialPort); 28 | return false; 29 | } 30 | struct termios options 31 | { 32 | }; 33 | 34 | // Even parity(115200 8N1): 35 | if (tcgetattr(fd, &options) != 0) 36 | { 37 | perror("SetupSerial 1"); 38 | } 39 | bzero(&options, sizeof(options)); 40 | options.c_cflag |= CLOCAL | CREAD; 41 | options.c_cflag &= ~CSIZE; 42 | 43 | options.c_cflag |= CS8; 44 | 45 | options.c_cflag &= ~PARENB; 46 | 47 | cfsetispeed(&options, B115200); 48 | cfsetospeed(&options, B115200); 49 | options.c_cflag &= ~CSTOPB; 50 | options.c_cc[VTIME] = 0; 51 | options.c_cc[VMIN] = 0; 52 | tcflush(fd, TCIFLUSH); 53 | if ((tcsetattr(fd, TCSANOW, &options)) != 0) 54 | { 55 | perror("com set error"); 56 | } 57 | serial_fds_.push_back(fd); 58 | ROS_INFO_STREAM("Successful to open " << serialPort << " port."); 59 | 60 | rc_hw::F1DataTransCh4 cmd_tran_; 61 | rc_hw::F6DataTransCh24 data_tran_; 62 | ros::Time last_writ_time_ = ros::Time::now(); 63 | rc_hw::SerialFlamMark serial_flam_mark_; 64 | u_int serial_data_pose_ = 0; 65 | cmd_tran_.f_data = 0.0; 66 | for (int i = 0; i < 6; i++) 67 | { 68 | data_tran_.f_data4[i] = 0.0; 69 | } 70 | serial_flam_mark_ = START; 71 | cmd_trans_.push_back(cmd_tran_); 72 | data_trans_.push_back(data_tran_); 73 | last_write_times_.push_back(last_writ_time_); 74 | serial_flam_marks_.push_back(serial_flam_mark_); 75 | serial_date_poses_.push_back(serial_data_pose_); 76 | 77 | return true; 78 | } 79 | 80 | void ActionManager::readAction(const ros::Time& time, const ros::Duration& period) 81 | { 82 | for (int id = 0; id < serial_fds_.size(); ++id) 83 | { 84 | if (action_command_values[id].calibration_state) 85 | { 86 | uint8_t buffer[1]; 87 | uint8_t num = 1; 88 | for (int i = 0; i < 28; ++i) 89 | { 90 | if (read(serial_fds_[id], buffer, 1) == 0) 91 | { 92 | break; 93 | } 94 | unPack(id, buffer[0]); 95 | } 96 | } 97 | } 98 | } 99 | 100 | void ActionManager::writeAction(const ros::Time& time, const ros::Duration& period) 101 | { 102 | for (int id = 0; id < serial_fds_.size(); id++) 103 | { 104 | // calibration_state default id True. 105 | if (!action_command_values[id].calibration_state) 106 | { 107 | // action calibration need cost 15min in absolute stillness 108 | // writCmd(id, "ACTR"); 109 | ROS_WARN_STREAM("Please keep this " << action_data_values[id].name 110 | << " in absolute stillness and continuous 15min."); 111 | action_command_values[id].calibration_state = true; 112 | } 113 | 114 | if (action_command_values[id].reset_state) 115 | { 116 | // action calibration need cost 15min in absolute stillness 117 | if (writCmd(id, "ACT0")) 118 | { 119 | ROS_INFO_STREAM("Rest " << action_data_values[id].name << " , starting over with data fusion."); 120 | action_command_values[id].calibration_state = false; 121 | } 122 | } 123 | 124 | if (action_command_values[id].update_yaw_state) 125 | { 126 | if (writCmd(id, "ACTZ", radToAng(action_command_values[id].update_yaw))) 127 | { 128 | ROS_INFO_STREAM("Yaw_angle of " << action_data_values[id].name << " has updated to " 129 | << action_command_values[id].update_yaw << "."); 130 | action_command_values[id].update_yaw_state = false; 131 | } 132 | } 133 | 134 | if (action_command_values[id].update_x_state) 135 | { 136 | if (writCmd(id, "ACTX", action_command_values[id].update_x * 1000.0)) 137 | { 138 | ROS_INFO_STREAM("Pose_x of " << action_data_values[id].name << " has updated to " 139 | << action_command_values[id].update_x << "."); 140 | action_command_values[id].update_x_state = false; 141 | } 142 | } 143 | 144 | if (action_command_values[id].update_y_state) 145 | { 146 | if (writCmd(id, "ACTY", action_command_values[id].update_y * 1000.0)) 147 | { 148 | ROS_INFO_STREAM("Pose_y of " << action_data_values[id].name << " has updated to " 149 | << action_command_values[id].update_y << "."); 150 | action_command_values[id].update_y_state = false; 151 | } 152 | } 153 | } 154 | } 155 | 156 | void ActionManager::unPack(const int id, const uint8_t data) 157 | { 158 | switch (serial_flam_marks_[id]) 159 | { 160 | case START: 161 | if (data == 0x0d) 162 | { 163 | serial_flam_marks_[id] = FIRST_HEARD; 164 | } 165 | else 166 | { 167 | serial_flam_marks_[id] = START; 168 | } 169 | break; 170 | case FIRST_HEARD: 171 | if (data == 0x0a) 172 | { 173 | serial_date_poses_[id] = 0; 174 | serial_flam_marks_[id] = SECOND_HEARD; 175 | } 176 | else if (data == 0x0d) 177 | { 178 | serial_flam_marks_[id] = FIRST_HEARD; 179 | } 180 | else 181 | { 182 | serial_flam_marks_[id] = START; 183 | } 184 | break; 185 | case SECOND_HEARD: 186 | data_trans_[id].ch_data[serial_date_poses_[id]] = data; 187 | serial_date_poses_[id]++; 188 | if (serial_date_poses_[id] >= 24) 189 | { 190 | serial_date_poses_[id] = 0; 191 | serial_flam_marks_[id] = DATA; 192 | } 193 | break; 194 | case DATA: 195 | if (data == 0x0a) 196 | { 197 | serial_flam_marks_[id] = FIRST_TAIL; 198 | } 199 | else 200 | { 201 | serial_flam_marks_[id] = START; 202 | } 203 | break; 204 | case FIRST_TAIL: 205 | if (data == 0x0d) // there state is SECOND_TAIL 206 | { 207 | action_data_values[id].yaw_angle = angToRad(data_trans_[id].f_data4[0]); 208 | action_data_values[id].pitch_angle = angToRad(data_trans_[id].f_data4[1]); 209 | action_data_values[id].roll_angle = angToRad(data_trans_[id].f_data4[2]); 210 | action_data_values[id].pose_x = data_trans_[id].f_data4[4] / 1000.0; 211 | action_data_values[id].pose_y = -1.0*data_trans_[id].f_data4[3] / 1000.0; 212 | action_data_values[id].yaw_acc = angToRad(data_trans_[id].f_data4[5]); 213 | } 214 | serial_flam_marks_[id] = START; 215 | break; 216 | default: 217 | serial_flam_marks_[id] = START; 218 | break; 219 | } 220 | } 221 | 222 | bool ActionManager::writCmd(const int id, std::string cmd) 223 | { 224 | if (write(serial_fds_[id], (char*)cmd.data(), cmd.length()) != -1) 225 | { 226 | ROS_DEBUG_STREAM("Successful write command to " << action_data_values[id].name << "."); 227 | } 228 | else 229 | { 230 | ROS_DEBUG_STREAM("Failed write command to " << action_data_values[id].name << "."); 231 | return false; 232 | } 233 | return true; 234 | } 235 | 236 | bool ActionManager::writCmd(const int id, const std::string cmd, const float data) 237 | { 238 | cmd_trans_[id].f_data = data; 239 | // update cmd need to delay 10ms 240 | if (ros::Time::now() - last_write_times_[id] >= ros::Duration(0.01)) 241 | { 242 | if (write(serial_fds_[id], (char*)cmd.data(), cmd.length()) != -1 || 243 | write(serial_fds_[id], (char*)cmd_trans_[id].ch_data, cmd.length()) != -1) 244 | { 245 | ROS_DEBUG_STREAM("Successful write command to " << action_data_values[id].name << "."); 246 | } 247 | else 248 | { 249 | ROS_DEBUG_STREAM("Failed write command to " << action_data_values[id].name << "."); 250 | return false; 251 | } 252 | } 253 | else 254 | { 255 | ROS_WARN_STREAM("action " << action_data_values[id].name << " is updating. Please try to again past 10ms."); 256 | } 257 | return true; 258 | } 259 | } // namespace rc_hw 260 | -------------------------------------------------------------------------------- /rc_hw/src/hardware_interface/gpio_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yezi on 2021/9/9. 3 | // 4 | 5 | #include 6 | 7 | namespace rc_hw 8 | { 9 | GpioManager::GpioManager() 10 | { 11 | } 12 | 13 | GpioManager::~GpioManager() 14 | { 15 | } 16 | void GpioManager::setGpioDirection(rc_control::GpioData gpioData) 17 | { 18 | std::string file = "/sys/class/gpio/gpio" + std::to_string(gpioData.pin) + "/direction"; 19 | int fd; 20 | fd = open(file.data(), O_WRONLY); 21 | if (fd == -1) 22 | { 23 | ROS_ERROR("[gpio]Unable to open %s", file.data()); 24 | } 25 | else 26 | { 27 | if (gpioData.type == rc_control::OUTPUT) 28 | { 29 | if (write(fd, "out", 3) != 3) 30 | { 31 | ROS_ERROR("[gpio]Failed to set direction of gpio%d", gpioData.pin); 32 | } 33 | } 34 | else 35 | { 36 | if (write(fd, "in", 2) != 2) 37 | { 38 | ROS_ERROR("[gpio]Failed to set direction of gpio%d", gpioData.pin); 39 | } 40 | } 41 | } 42 | close(fd); 43 | } 44 | 45 | void GpioManager::readGpio() 46 | { 47 | for (auto iter = gpio_state_values.begin(); iter != gpio_state_values.end(); iter++) 48 | { 49 | if (iter->type == rc_control::INPUT) 50 | { 51 | std::string file = "/sys/class/gpio/gpio" + std::to_string(iter->pin) + "/value"; 52 | FILE* fp = fopen(file.c_str(), "r"); 53 | if (fp == NULL) 54 | { 55 | ROS_ERROR("[gpio]Unable to read /sys/class/gpio/gpio%d/value", iter->pin); 56 | } 57 | else 58 | { 59 | char state = fgetc(fp); 60 | bool value = (state == 0x31); 61 | *iter->value = value; 62 | fclose(fp); 63 | } 64 | } 65 | } 66 | } 67 | 68 | void GpioManager::writeGpio() 69 | { 70 | char buffer[1] = { '1' }; 71 | for (auto iter : gpio_command_values) 72 | { 73 | std::string file = "/sys/class/gpio/gpio" + std::to_string(iter.pin) + "/value"; 74 | int fd = open(file.c_str(), O_WRONLY); 75 | if (fd == -1) 76 | { 77 | ROS_ERROR("[gpio]Unable to write /sys/class/gpio/gpio%i/value", iter.pin); 78 | } 79 | else 80 | { 81 | lseek(fd, 0, SEEK_SET); 82 | if (*iter.value) 83 | { 84 | buffer[0] = '1'; 85 | int ref = write(fd, buffer, 1); 86 | if (ref == -1) 87 | ROS_ERROR("[GPIO]Failed to write to gpio%d.", iter.pin); 88 | } 89 | else 90 | { 91 | buffer[0] = '0'; 92 | int ref = write(fd, buffer, 1); 93 | if (ref == -1) 94 | ROS_ERROR("[GPIO]Failed to write to gpio%d.", iter.pin); 95 | } 96 | } 97 | close(fd); 98 | } 99 | } 100 | } // namespace rc_hw 101 | -------------------------------------------------------------------------------- /rc_hw/src/hardware_interface/hardware_interface.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 12/21/20. 36 | // 37 | 38 | #include "rc_hw/hardware_interface/hardware_interface.h" 39 | 40 | #include 41 | 42 | namespace rc_hw 43 | { 44 | bool RcRobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) 45 | { 46 | XmlRpc::XmlRpcValue xml_rpc_value; 47 | // Parse actuator coefficient specified by user (stored on ROS parameter server) 48 | if (!robot_hw_nh.getParam("actuator_coefficient", xml_rpc_value)) 49 | ROS_WARN("No actuator coefficient specified"); 50 | else if (!parseActCoeffs(xml_rpc_value)) 51 | return false; 52 | // Parse actuator specified by user (stored on ROS parameter server) 53 | if (!robot_hw_nh.getParam("actuators", xml_rpc_value)) 54 | ROS_WARN("No actuator specified"); 55 | else if (!parseActData(xml_rpc_value, robot_hw_nh)) 56 | return false; 57 | // Parse imus specified by user (stored on ROS parameter server) 58 | if (!robot_hw_nh.getParam("imus", xml_rpc_value)) 59 | ROS_WARN("No imu specified"); 60 | else if (!parseImuData(xml_rpc_value, robot_hw_nh)) 61 | return false; 62 | // Parse gpios specified by user (stored on ROS parameter server) 63 | if (!robot_hw_nh.getParam("gpios", xml_rpc_value)) 64 | ROS_WARN("No gpio specified"); 65 | else if (!parseGpioData(xml_rpc_value, robot_hw_nh)) 66 | return false; 67 | // Parse actions specified by user (stored on ROS parameter server) 68 | if (!robot_hw_nh.getParam("actions", xml_rpc_value)) 69 | ROS_WARN("No actions specified"); 70 | else if (!parseActionData(xml_rpc_value, robot_hw_nh)) 71 | return false; 72 | 73 | // CAN Bus 74 | if (!robot_hw_nh.getParam("bus", xml_rpc_value)) 75 | ROS_WARN("No bus specified"); 76 | // DT35-laser 77 | if (!robot_hw_nh.getParam("dt35laser", xml_rpc_value)) 78 | ROS_WARN("No dt35laser specified"); 79 | else if (!parseDT35Data(xml_rpc_value,robot_hw_nh)) 80 | return false; 81 | else if (xml_rpc_value.getType() == XmlRpc::XmlRpcValue::TypeArray) 82 | { 83 | ROS_ASSERT(xml_rpc_value[0].getType() == XmlRpc::XmlRpcValue::TypeString); 84 | for (int i = 0; i < xml_rpc_value.size(); ++i) 85 | { 86 | std::string bus_name = xml_rpc_value[i]; 87 | if (bus_name.find("can") != std::string::npos) 88 | can_buses_.push_back(new CanBus(bus_name, 89 | CanDataPtr{ 90 | .type2act_coeffs_ = &type2act_coeffs_, 91 | .id2act_data_ = &bus_id2act_data_[bus_name], 92 | .id2imu_data_ = &bus_id2imu_data_[bus_name], 93 | }, 94 | thread_priority_)); 95 | else 96 | ROS_ERROR_STREAM("Unknown bus: " << bus_name); 97 | } 98 | } 99 | 100 | if (!loadUrdf(root_nh)) 101 | { 102 | ROS_ERROR("Error occurred while setting up urdf"); 103 | return false; 104 | } 105 | // Initialize transmission 106 | if (!setupTransmission(root_nh)) 107 | { 108 | ROS_ERROR("Error occurred while setting up transmission"); 109 | return false; 110 | } 111 | // Initialize joint limit 112 | if (!setupJointLimit(root_nh)) 113 | { 114 | ROS_ERROR("Error occurred while setting up joint limit"); 115 | return false; 116 | } 117 | 118 | // Other Interface 119 | registerInterface(&robot_state_interface_); 120 | 121 | actuator_state_pub_.reset( 122 | new realtime_tools::RealtimePublisher(root_nh, "/actuator_states", 100)); 123 | 124 | return true; 125 | } 126 | 127 | void RcRobotHW::read(const ros::Time& time, const ros::Duration& period) 128 | { 129 | for (auto bus : can_buses_) 130 | bus->read(time); 131 | for (auto& id2act_datas : bus_id2act_data_) 132 | for (auto& act_data : id2act_datas.second) 133 | { 134 | try 135 | { // Duration will be out of dual 32-bit range while motor failure 136 | act_data.second.halted = (time - act_data.second.stamp).toSec() > 0.1 || act_data.second.temp > 99; 137 | } 138 | catch (std::runtime_error& ex) 139 | { 140 | } 141 | if (act_data.second.halted) 142 | { 143 | act_data.second.seq = 0; 144 | act_data.second.vel = 0; 145 | act_data.second.effort = 0; 146 | act_data.second.calibrated = false; // set the actuator no calibrated 147 | } 148 | } 149 | if (is_actuator_specified_) 150 | act_to_jnt_state_->propagate(); 151 | // Set all cmd to zero to avoid crazy soft limit oscillation when not controller loaded 152 | for (auto effort_joint_handle : effort_joint_handles_) 153 | effort_joint_handle.setCommand(0.); 154 | // Gpio read 155 | gpio_manager_.readGpio(); 156 | action_manager_.readAction(time, period); 157 | // DT35 read 158 | dt35_laser_.readDT35_laser(time,period); 159 | } 160 | void RcRobotHW::write(const ros::Time& time, const ros::Duration& period) 161 | { 162 | if (is_actuator_specified_) 163 | { 164 | // Propagate without joint limits 165 | jnt_to_act_effort_->propagate(); 166 | // Save commanded effort before enforceLimits 167 | for (auto& id2act_datas : bus_id2act_data_) 168 | for (auto& act_data : id2act_datas.second) 169 | act_data.second.cmd_effort = act_data.second.exe_effort; 170 | // enforceLimits will limit cmd_effort into suitable value https://github.com/ros-controls/ros_control/wiki/joint_limits_interface 171 | effort_jnt_saturation_interface_.enforceLimits(period); 172 | effort_jnt_soft_limits_interface_.enforceLimits(period); 173 | // Propagate with joint limits 174 | jnt_to_act_effort_->propagate(); 175 | // Restore the cmd_effort for the calibrating joint 176 | for (auto& id2act_datas : bus_id2act_data_) 177 | for (auto& act_data : id2act_datas.second) 178 | if (act_data.second.need_calibration && !act_data.second.calibrated) 179 | act_data.second.exe_effort = act_data.second.cmd_effort; 180 | } 181 | for (auto& bus : can_buses_) 182 | bus->write(); 183 | // Gpio write 184 | gpio_manager_.writeGpio(); 185 | publishActuatorState(time); 186 | // @TODO because some reason? Action will stop output info when send cmd 187 | // action_manager_.writeAction(time, period); 188 | } 189 | 190 | void RcRobotHW::setCanBusThreadPriority(int thread_priority) 191 | { 192 | thread_priority_ = thread_priority; 193 | } 194 | 195 | void RcRobotHW::publishActuatorState(const ros::Time& time) 196 | { 197 | if (!is_actuator_specified_) 198 | return; 199 | if (last_publish_time_ + ros::Duration(1.0 / 100.0) < time) 200 | { 201 | if (actuator_state_pub_->trylock()) 202 | { 203 | rc_msgs::ActuatorState actuator_state; 204 | for (const auto& id2act_datas : bus_id2act_data_) 205 | for (const auto& act_data : id2act_datas.second) 206 | { 207 | actuator_state.stamp.push_back(act_data.second.stamp); 208 | actuator_state.name.push_back(act_data.second.name); 209 | actuator_state.type.push_back(act_data.second.type); 210 | actuator_state.bus.push_back(id2act_datas.first); 211 | actuator_state.id.push_back(act_data.first); 212 | actuator_state.halted.push_back(act_data.second.halted); 213 | actuator_state.need_calibration.push_back(act_data.second.need_calibration); 214 | actuator_state.calibrated.push_back(act_data.second.calibrated); 215 | actuator_state.calibration_reading.push_back(act_data.second.calibration_reading); 216 | actuator_state.position_raw.push_back(act_data.second.q_raw); 217 | actuator_state.velocity_raw.push_back(act_data.second.qd_raw); 218 | actuator_state.temperature.push_back(act_data.second.temp); 219 | actuator_state.circle.push_back(act_data.second.q_circle); 220 | actuator_state.last_position_raw.push_back(act_data.second.q_last); 221 | actuator_state.frequency.push_back(act_data.second.frequency); 222 | actuator_state.position.push_back(act_data.second.pos); 223 | actuator_state.velocity.push_back(act_data.second.vel); 224 | actuator_state.effort.push_back(act_data.second.effort); 225 | actuator_state.commanded_effort.push_back(act_data.second.cmd_effort); 226 | actuator_state.executed_effort.push_back(act_data.second.exe_effort); 227 | actuator_state.offset.push_back(act_data.second.offset); 228 | } 229 | actuator_state_pub_->msg_ = actuator_state; 230 | actuator_state_pub_->unlockAndPublish(); 231 | last_publish_time_ = time; 232 | } 233 | } 234 | } 235 | 236 | } // namespace rc_hw 237 | -------------------------------------------------------------------------------- /rc_hw/src/hardware_interface/socketcan.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 3/3/21. 36 | // 37 | #include "rc_hw/hardware_interface/socketcant.h" 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace can 44 | { 45 | /* ref: 46 | * https://github.com/JCube001/socketcan-demo 47 | * http://blog.mbedded.ninja/programming/operating-systems/linux/how-to-use-socketcan-with-c-in-linux 48 | * https://github.com/linux-can/can-utils/blob/master/candump.c 49 | */ 50 | 51 | SocketCAN::~SocketCAN() 52 | { 53 | if (this->isOpen()) 54 | this->close(); 55 | } 56 | 57 | bool SocketCAN::open(const std::string& interface, boost::function handler, 58 | int thread_priority) 59 | { 60 | reception_handler = std::move(handler); 61 | // Request a socket 62 | sock_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); 63 | if (sock_fd_ == -1) 64 | { 65 | ROS_ERROR("Error: Unable to create a CAN socket"); 66 | return false; 67 | } 68 | char name[16] = {}; // avoid stringop-truncation 69 | strncpy(name, interface.c_str(), interface.size()); 70 | strncpy(interface_request_.ifr_name, name, IFNAMSIZ); 71 | // Get the index of the network interface 72 | if (ioctl(sock_fd_, SIOCGIFINDEX, &interface_request_) == -1) 73 | { 74 | ROS_ERROR("Unable to select CAN interface %s: I/O control error", name); 75 | // Invalidate unusable socket 76 | close(); 77 | return false; 78 | } 79 | // Bind the socket to the network interface 80 | address_.can_family = AF_CAN; 81 | address_.can_ifindex = interface_request_.ifr_ifindex; 82 | int rc = bind(sock_fd_, reinterpret_cast(&address_), sizeof(address_)); 83 | if (rc == -1) 84 | { 85 | ROS_ERROR("Failed to bind socket to %s network interface", name); 86 | close(); 87 | return false; 88 | } 89 | // Start a separate, event-driven thread for frame reception 90 | return startReceiverThread(thread_priority); 91 | } 92 | 93 | void SocketCAN::close() 94 | { 95 | terminate_receiver_thread_ = true; 96 | while (receiver_thread_running_) 97 | ; 98 | 99 | if (!isOpen()) 100 | return; 101 | // Close the file descriptor for our socket 102 | ::close(sock_fd_); 103 | sock_fd_ = -1; 104 | } 105 | 106 | bool SocketCAN::isOpen() const 107 | { 108 | return (sock_fd_ != -1); 109 | } 110 | 111 | void SocketCAN::write(can_frame* frame) const 112 | { 113 | if (!isOpen()) 114 | { 115 | ROS_ERROR_THROTTLE(5., "Unable to write: Socket %s not open", interface_request_.ifr_name); 116 | return; 117 | } 118 | if (::write(sock_fd_, frame, sizeof(can_frame)) == -1) 119 | ROS_DEBUG_THROTTLE(5., "Unable to write: The %s tx buffer may be full", interface_request_.ifr_name); 120 | } 121 | 122 | static void* socketcan_receiver_thread(void* argv) 123 | { 124 | /* 125 | * The first and only argument to this function 126 | * is the pointer to the object, which started the thread. 127 | */ 128 | auto* sock = (SocketCAN*)argv; 129 | // Holds the set of descriptors, that 'select' shall monitor 130 | fd_set descriptors; 131 | // Highest file descriptor in set 132 | int maxfd = sock->sock_fd_; 133 | // How long 'select' shall wait before returning with timeout 134 | struct timeval timeout 135 | { 136 | }; 137 | // Buffer to store incoming frame 138 | can_frame rx_frame{}; 139 | // Run until termination signal received 140 | sock->receiver_thread_running_ = true; 141 | while (!sock->terminate_receiver_thread_) 142 | { 143 | timeout.tv_sec = 1.; // Should be set each loop 144 | // Clear descriptor set 145 | FD_ZERO(&descriptors); 146 | // Add socket descriptor 147 | FD_SET(sock->sock_fd_, &descriptors); 148 | // Wait until timeout or activity on any descriptor 149 | if (select(maxfd + 1, &descriptors, nullptr, nullptr, &timeout)) 150 | { 151 | size_t len = read(sock->sock_fd_, &rx_frame, CAN_MTU); 152 | if (len < 0) 153 | continue; 154 | if (sock->reception_handler != nullptr) 155 | sock->reception_handler(rx_frame); 156 | } 157 | } 158 | sock->receiver_thread_running_ = false; 159 | return nullptr; 160 | } 161 | 162 | bool SocketCAN::startReceiverThread(int thread_priority) 163 | { 164 | // Frame reception is accomplished in a separate, event-driven thread. 165 | // See also: https://www.thegeekstuff.com/2012/04/create-threads-in-linux/ 166 | terminate_receiver_thread_ = false; 167 | int rc = pthread_create(&receiver_thread_id_, nullptr, &socketcan_receiver_thread, this); 168 | if (rc != 0) 169 | { 170 | ROS_ERROR("Unable to start receiver thread"); 171 | return false; 172 | } 173 | ROS_INFO("Successfully started receiver thread with ID %lu", receiver_thread_id_); 174 | sched_param sched{ .sched_priority = thread_priority }; 175 | pthread_setschedparam(receiver_thread_id_, SCHED_FIFO, &sched); 176 | return true; 177 | } 178 | 179 | } // namespace can 180 | -------------------------------------------------------------------------------- /rc_hw/src/rc_hw.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 12/27/20. 36 | // 37 | 38 | #include "rc_hw/control_loop.h" 39 | 40 | int main(int argc, char** argv) 41 | { 42 | ros::init(argc, argv, "rc_hw"); 43 | ros::NodeHandle nh; 44 | 45 | // Run the hardware interface node 46 | // ------------------------------- 47 | 48 | // We run the ROS loop in a separate thread as external calls, such 49 | // as service callbacks loading controllers, can block the (main) control loop 50 | 51 | ros::AsyncSpinner spinner(2); 52 | spinner.start(); 53 | 54 | try 55 | { 56 | // Create the hardware interface specific to your robot 57 | std::shared_ptr rc_hw_hw_interface = std::make_shared(); 58 | 59 | // Start the control loop 60 | rc_hw::RcRobotHWLoop control_loop(nh, rc_hw_hw_interface); 61 | 62 | // Wait until shutdown signal received 63 | ros::waitForShutdown(); 64 | } 65 | catch (const ros::Exception& e) 66 | { 67 | ROS_FATAL_STREAM("Error in the hardware interface:\n" 68 | << "\t" << e.what()); 69 | return 1; 70 | } 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /rc_hw/test/test_hw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | -------------------------------------------------------------------------------- /rc_hw/test/test_hw.yaml: -------------------------------------------------------------------------------- 1 | rc_hw: 2 | # bus: 3 | # - can0 4 | # - can1 5 | loop_frequency: 1000 6 | cycle_time_error_threshold: 0.001 7 | thread_priority: 95 8 | # actions: 9 | # location_action: # action names 10 | # serial_port: "/dev/ttyUSB0" 11 | 12 | # Configurations of the actuators 13 | # actuators: 14 | # mit_joint_motor: 15 | # bus: can0 16 | # id: 0x001 17 | # type: cheetah 18 | # Configurations of the actuators 19 | # actuators: 20 | # right_front_wheel_motor: 21 | # bus: can0 22 | # id: 0x201 23 | # type: rm_3508 24 | # lp_cutoff_frequency: 60 25 | dt35laser: 26 | moudle1: 27 | serial_port: "/dev/ttyUSB0" 28 | 29 | -------------------------------------------------------------------------------- /rc_ibus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rc_ibus) 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 | roslint 17 | rc_common 18 | ) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | ## The catkin_package macro generates cmake config files for your package 24 | ## Declare things to be passed to dependent projects 25 | ## INCLUDE_DIRS: uncomment this if your package contains header files 26 | ## LIBRARIES: libraries you create in this project that dependent projects also need 27 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 28 | ## DEPENDS: system dependencies of this project that dependent projects also need 29 | catkin_package( 30 | INCLUDE_DIRS 31 | include 32 | ## This is only necessary because Eigen3 sets a non-standard EIGEN3_INCLUDE_DIR variable 33 | CATKIN_DEPENDS 34 | roscpp 35 | rc_common 36 | DEPENDS 37 | ## find_package(Eigen3) provides a non standard EIGEN3_INCLUDE_DIR instead of Eigen3_INCLUDE_DIRS. 38 | ## Therefore, the DEPEND does not work as expected and we need to add the directory to the INCLUDE_DIRS 39 | # Eigen3 40 | 41 | ## Boost is not part of the DEPENDS since it is only used in source files, 42 | ## Dependees do not depend on Boost when they depend on this package. 43 | ) 44 | 45 | ########### 46 | ## Build ## 47 | ########### 48 | 49 | ## Specify additional locations of header files 50 | ## Your package locations should be listed before other locations 51 | include_directories( 52 | include 53 | ${catkin_INCLUDE_DIRS} 54 | ) 55 | 56 | ## Declare cpp executables 57 | add_executable(${PROJECT_NAME} 58 | src/ibus_node.cpp 59 | src/ibus.cpp 60 | ) 61 | 62 | ## Specify libraries to link executable targets against 63 | target_link_libraries(${PROJECT_NAME} 64 | ${catkin_LIBRARIES} 65 | ) 66 | 67 | ############# 68 | ## Install ## 69 | ############# 70 | 71 | # Mark executables and/or libraries for installation 72 | install( 73 | TARGETS ${PROJECT_NAME} 74 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 77 | ) 78 | 79 | # Mark cpp header files for installation 80 | install( 81 | DIRECTORY include/${PROJECT_NAME}/ 82 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 83 | FILES_MATCHING PATTERN "*.h" 84 | ) 85 | 86 | # Mark other files for installation 87 | install( 88 | DIRECTORY config 89 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 90 | ) 91 | 92 | roslint_cpp() 93 | -------------------------------------------------------------------------------- /rc_ibus/config/rc_ibus.yaml: -------------------------------------------------------------------------------- 1 | rc_hw: 2 | rc_ibus: 3 | # ls /dev/tty* 4 | serial_port: "/dev/usbDbus" 5 | -------------------------------------------------------------------------------- /rc_ibus/doc/fs_i6s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gdut-robocon/rc_control/d1c64d02ae4fa32dbecc409aef2b2d622f059750/rc_ibus/doc/fs_i6s.png -------------------------------------------------------------------------------- /rc_ibus/include/rc_ibus/ibus.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/29. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace rc_ibus 11 | { 12 | typedef struct 13 | { 14 | int16_t ch[10]; 15 | 16 | int16_t checksum_ibus; 17 | } IBusData_t; 18 | 19 | class IBus 20 | { 21 | public: 22 | IBus(); 23 | ~IBus(); 24 | void init(const char* serial); 25 | void getData(rc_msgs::IbusData* i_bus_data); 26 | void read(); 27 | 28 | private: 29 | IBusData_t i_bus_data_{}; 30 | int port_{}; 31 | int16_t buff_[32]{}; 32 | uint8_t count = 0; 33 | uint8_t data_i = 0; 34 | bool is_update_ = false; 35 | void unpack(const uint8_t data); 36 | }; 37 | } // namespace rc_ibus 38 | -------------------------------------------------------------------------------- /rc_ibus/include/rc_ibus/ibus_node.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/29. 3 | // 4 | 5 | #pragma once 6 | 7 | #include "ibus.h" 8 | #include 9 | #include 10 | namespace rc_ibus { 11 | class IBusNode { 12 | private: 13 | ros::NodeHandle nh_; 14 | ros::Publisher ibus_pub_; 15 | std::string serial_port_; 16 | rc_msgs::IbusData Ibus_cmd_; 17 | rc_ibus::IBus ibus_{}; 18 | 19 | public: 20 | IBusNode(); 21 | 22 | ~IBusNode() = default; 23 | 24 | void run(); 25 | }; 26 | }//namespace rc_ibus -------------------------------------------------------------------------------- /rc_ibus/launch/rc_ibus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /rc_ibus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rc_ibus 4 | 0.0.0 5 | The rc_ibus package 6 | myx 7 | BSD 8 | 9 | 10 | catkin 11 | 12 | 13 | roscpp 14 | roslint 15 | 16 | rc_common 17 | 18 | 19 | -------------------------------------------------------------------------------- /rc_ibus/src/ibus.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/29. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | #include /* File control definitions */ 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | extern int ioctl(int __fd, unsigned long int __request, ...) throw(); 14 | } 15 | 16 | namespace rc_ibus 17 | { 18 | IBus::IBus() 19 | { 20 | } 21 | IBus::~IBus() 22 | { 23 | } 24 | void IBus::init(const char* serial) 25 | { 26 | int fd = open(serial, O_RDWR | O_NOCTTY | O_SYNC); 27 | if (fd == -1) 28 | { 29 | ROS_ERROR("[rc_ibus] Unable to open ibus. serial port: %s",serial); 30 | return; 31 | } 32 | struct termios options 33 | { 34 | }; 35 | 36 | // Even parity(115200 8N1): 37 | if (tcgetattr(fd, &options) != 0) 38 | { 39 | perror("SetupSerial 1"); 40 | } 41 | bzero(&options, sizeof(options)); 42 | options.c_cflag |= CLOCAL | CREAD; 43 | options.c_cflag &= ~CSIZE; 44 | 45 | options.c_cflag |= CS8; 46 | 47 | options.c_cflag &= ~PARENB; 48 | 49 | cfsetispeed(&options, B115200); 50 | cfsetospeed(&options, B115200); 51 | options.c_cflag &= ~CSTOPB; 52 | options.c_cc[VTIME] = 0; 53 | options.c_cc[VMIN] = 0; 54 | tcflush(fd, TCIFLUSH); 55 | if ((tcsetattr(fd, TCSANOW, &options)) != 0) 56 | { 57 | perror("com set error"); 58 | } 59 | ROS_INFO_STREAM("Successful to open " << serial << " port."); 60 | port_ = fd; 61 | } 62 | 63 | void IBus::read() 64 | { 65 | uint8_t read_byte; 66 | int timeout = 0; // time out of one package 67 | while (timeout < 5) 68 | { 69 | if (count == 3) 70 | { 71 | count = 0; 72 | return; 73 | } 74 | // Read a byte // 75 | size_t n = ::read(port_, &read_byte, sizeof(read_byte)); 76 | 77 | if (n == 0) 78 | { 79 | timeout++; 80 | } 81 | else if (n == 1) 82 | { 83 | unpack(read_byte); 84 | } 85 | } 86 | } 87 | 88 | void IBus::unpack(const uint8_t data) 89 | { 90 | switch (count) 91 | { 92 | case 0: 93 | if (data == 0x20) 94 | { 95 | data_i = 0; 96 | buff_[data_i] = 0x20; 97 | count++; 98 | } 99 | else 100 | { 101 | count = 0; 102 | } 103 | break; 104 | case 1: 105 | if (data == 0x40) 106 | { 107 | data_i = 1; 108 | buff_[data_i] = 0x40; 109 | count++; 110 | } 111 | else 112 | { 113 | count = 0; 114 | } 115 | break; 116 | case 2: 117 | data_i++; 118 | buff_[data_i] = data; 119 | i_bus_data_.checksum_ibus = (uint16_t)(buff_[30] << 8 | buff_[31]); 120 | if (data_i >= 31) 121 | { 122 | count++; 123 | data_i = 0; 124 | 125 | for (int i = 0; i < 10; ++i) 126 | { 127 | i_bus_data_.ch[i] = (uint16_t)((buff_[i * 2 + 3] & 0x0F) << 8 | buff_[i * 2 + 2]); 128 | } 129 | is_update_ = true; 130 | } 131 | break; 132 | default: 133 | count = 0; 134 | break; 135 | } 136 | } 137 | 138 | void IBus::getData(rc_msgs::IbusData* i_bus_cmd) 139 | { 140 | if (is_update_) 141 | { 142 | i_bus_cmd->stamp = ros::Time::now(); 143 | 144 | // ch_r_* 145 | for (int i = 0; i < 4; ++i) 146 | { 147 | if (abs(i_bus_data_.ch[i] - 1500) <= 10) 148 | i_bus_data_.ch[i] = 1500; 149 | 150 | if (abs(i_bus_data_.ch[i] - 1500) > 500) 151 | { 152 | return; 153 | } 154 | } 155 | i_bus_cmd->ch_r_x = static_cast((i_bus_data_.ch[0] - 1500.0) / 500.0); 156 | i_bus_cmd->ch_r_y = static_cast((i_bus_data_.ch[1] - 1500.0) / 500.0); 157 | i_bus_cmd->ch_l_y = static_cast((i_bus_data_.ch[2] - 1500.0) / 500.0); 158 | i_bus_cmd->ch_l_x = static_cast((i_bus_data_.ch[3] - 1500.0) / 500.0); 159 | 160 | // i_bus_cmd->sw_a i_bus_data_.ch[4] 161 | if (abs(i_bus_data_.ch[4] - 1000) < 5) // 1000 162 | { 163 | i_bus_cmd->sw_a = i_bus_cmd->UP; 164 | } 165 | else if (abs(i_bus_data_.ch[4] - 2000) < 5) // 2000 166 | { 167 | i_bus_cmd->sw_a = i_bus_cmd->DOWN; 168 | } 169 | 170 | // i_bus_cmd->sw_a i_bus_data_.ch[5] 171 | if (abs(i_bus_data_.ch[5] - 1000) < 5) 172 | { 173 | i_bus_cmd->sw_b = i_bus_cmd->UP; 174 | } 175 | else if (abs(i_bus_data_.ch[5] - 1500) < 5) 176 | { 177 | i_bus_cmd->sw_b = i_bus_cmd->MID; 178 | } 179 | else if (abs(i_bus_data_.ch[5] - 2000) < 5) 180 | { 181 | i_bus_cmd->sw_b = i_bus_cmd->DOWN; 182 | } 183 | 184 | // i_bus_cmd->sw_c i_bus_data_.ch[6] 185 | if (abs(i_bus_data_.ch[6] - 1000) < 5) 186 | { 187 | i_bus_cmd->sw_c = i_bus_cmd->UP; 188 | } 189 | else if (abs(i_bus_data_.ch[6] - 1500) < 5) 190 | { 191 | i_bus_cmd->sw_c = i_bus_cmd->MID; 192 | } 193 | else if (abs(i_bus_data_.ch[6] - 2000) < 5) 194 | { 195 | i_bus_cmd->sw_c = i_bus_cmd->DOWN; 196 | } 197 | 198 | // i_bus_cmd->sw_d i_bus_data_.ch[7] 199 | if (abs(i_bus_data_.ch[7] - 1000) < 5) 200 | { 201 | i_bus_cmd->sw_d = i_bus_cmd->UP; 202 | } 203 | else if (abs(i_bus_data_.ch[7] - 2000) < 5) 204 | { 205 | i_bus_cmd->sw_d = i_bus_cmd->DOWN; 206 | } 207 | 208 | i_bus_cmd->vr_a = static_cast((i_bus_data_.ch[8] - 1500.0) / 500.0); 209 | i_bus_cmd->vr_b = static_cast((i_bus_data_.ch[9] - 1500.0) / 500.0); 210 | 211 | is_update_ = false; 212 | } 213 | } 214 | } // namespace rc_ibus -------------------------------------------------------------------------------- /rc_ibus/src/ibus_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by myx on 2022/11/29. 3 | // 4 | #include "rc_ibus/ibus_node.h" 5 | 6 | int main(int argc, char** argv) 7 | { 8 | ros::init(argc, argv, "rc_ibus"); 9 | rc_ibus::IBusNode ibus_node; 10 | ros::Rate loop_rate(1000); 11 | while (ros::ok()) 12 | { 13 | ibus_node.run(); 14 | loop_rate.sleep(); 15 | } 16 | return 0; 17 | } 18 | 19 | namespace rc_ibus 20 | { 21 | IBusNode::IBusNode() { 22 | ibus_pub_ = nh_.advertise("ibus_data", 1); 23 | ros::NodeHandle nh_ibus("rc_hw/rc_ibus"); 24 | nh_ibus.param("serial_port", serial_port_, "/dev/ttyUSB0"); 25 | ibus_.init(serial_port_.data()); 26 | } 27 | 28 | void IBusNode::run() { 29 | ibus_.read(); 30 | ibus_.getData(&Ibus_cmd_); 31 | ibus_pub_.publish(Ibus_cmd_); 32 | } 33 | } -------------------------------------------------------------------------------- /rc_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rc_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | actionlib 9 | actionlib_msgs 10 | ) 11 | 12 | add_message_files( 13 | FILES 14 | ActuatorState.msg 15 | GpioData.msg 16 | LpData.msg 17 | ActionData.msg 18 | ChassisCmd.msg 19 | IbusData.msg 20 | GimbalCmd.msg 21 | GimbalDesError.msg 22 | ShooterCmd.msg 23 | ) 24 | 25 | add_service_files( 26 | FILES 27 | ActionCmd.srv 28 | # ContinousDetectorSwitch.srv 29 | # StatusChange.srv 30 | # CameraStatus.srv 31 | # EnableImuTrigger.srv 32 | ) 33 | 34 | #add_action_files( 35 | # FILES 36 | # Engineer.action 37 | #) 38 | 39 | # Generate added messages and services with any dependencies listed here 40 | generate_messages( 41 | DEPENDENCIES 42 | std_msgs 43 | geometry_msgs 44 | actionlib_msgs 45 | ) 46 | 47 | catkin_package( 48 | CATKIN_DEPENDS 49 | message_runtime 50 | std_msgs 51 | geometry_msgs 52 | actionlib 53 | actionlib_msgs 54 | ) 55 | 56 | -------------------------------------------------------------------------------- /rc_msgs/msg/ActionData.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 yaw_angle 3 | float64 pitch_angle 4 | float64 roll_angle 5 | float64 pose_x 6 | float64 pose_y 7 | float64 yaw_acc -------------------------------------------------------------------------------- /rc_msgs/msg/ActuatorState.msg: -------------------------------------------------------------------------------- 1 | # This message contains the state of an actuator 2 | # An actuator contains a motor and an encoder, and is connected 3 | # to a joint by a transmission 4 | 5 | # The time at which this actuator state was measured 6 | time[] stamp 7 | 8 | # The name of the actuator 9 | string[] name 10 | 11 | # The type of the actuator 12 | string[] type 13 | 14 | # The CAN bus 15 | string[] bus 16 | 17 | # The CAN id 18 | int32[] id 19 | 20 | # Indicates if the motor is halted. A motor can be halted because of a voltage or temperature problem 21 | bool[] halted 22 | 23 | # Need calibration 24 | bool[] need_calibration 25 | 26 | # Indicates if the motor is calibrated. A motor will be not calibrated when recover from halted 27 | bool[] calibrated 28 | 29 | # The value of the calibration reading: low (false) or high (true) 30 | bool[] calibration_reading 31 | 32 | # The encoder raw position, represented by the number of encoder ticks 33 | uint16[] position_raw 34 | 35 | # The encoder velocity, represented by rpm 36 | int16[] velocity_raw 37 | 38 | # The temperature of the motor, represented by c1elsius 39 | uint8[] temperature 40 | 41 | # The circle of absolute encoder 42 | int64[] circle 43 | 44 | # The last encoder raw position, represented by the number of encoder ticks 45 | uint16[] last_position_raw 46 | 47 | float64[] frequency 48 | 49 | # The encoder position in radians 50 | float64[] position 51 | 52 | # The encoder velocity in radians per second 53 | float64[] velocity 54 | 55 | # The last effort that was measured by the actuator 56 | float64[] effort 57 | 58 | # The last effort command that was requested without limit 59 | float64[] commanded_effort 60 | 61 | # The last effort command that was requested with limit 62 | float64[] executed_effort 63 | 64 | # The angular offset (in radians) that is added to the encoder reading, 65 | # to get to the position of the actuator. This number is computed when the referece 66 | # sensor is triggered during the calibration phase 67 | float64[] offset 68 | -------------------------------------------------------------------------------- /rc_msgs/msg/ChassisCmd.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Accel accel 2 | string command_source_frame 3 | time stamp 4 | -------------------------------------------------------------------------------- /rc_msgs/msg/GimbalCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 RATE = 0 2 | uint8 NORMAL = 1 3 | 4 | time stamp 5 | uint8 mode 6 | 7 | #RATE 8 | float64 rate_yaw 9 | float64 rate_pitch 10 | 11 | #NORMAL 12 | float64 yaw_target_pos 13 | float64 pitch_target_pos 14 | 15 | #DIRECT 16 | geometry_msgs/PointStamped target_pos 17 | -------------------------------------------------------------------------------- /rc_msgs/msg/GimbalDesError.msg: -------------------------------------------------------------------------------- 1 | float64 error 2 | 3 | time stamp -------------------------------------------------------------------------------- /rc_msgs/msg/GpioData.msg: -------------------------------------------------------------------------------- 1 | string[] gpio_name 2 | bool[] gpio_state 3 | string[] gpio_type 4 | std_msgs/Header header 5 | -------------------------------------------------------------------------------- /rc_msgs/msg/IbusData.msg: -------------------------------------------------------------------------------- 1 | uint8 UP = 1 2 | uint8 DOWN = 2 3 | uint8 MID = 3 4 | 5 | uint8 sw_a 6 | uint8 sw_b 7 | uint8 sw_c 8 | uint8 sw_d 9 | 10 | float64 vr_a 11 | float64 vr_b 12 | 13 | float64 ch_l_x 14 | float64 ch_l_y 15 | float64 ch_r_x 16 | float64 ch_r_y 17 | 18 | bool key_l 19 | bool key_r 20 | 21 | time stamp -------------------------------------------------------------------------------- /rc_msgs/msg/LpData.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 real 3 | float64 filtered 4 | -------------------------------------------------------------------------------- /rc_msgs/msg/ShooterCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 Same=0 2 | uint8 diff=1 3 | 4 | time stamp 5 | uint8 mode 6 | bool master 7 | float64 speed 8 | 9 | float64 push_vel 10 | float64 left_ring_vel 11 | float64 right_ring_vel 12 | -------------------------------------------------------------------------------- /rc_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rc_msgs 4 | 0.0.0 5 | The rc_msgs package provides all the messages for all kind of robot 6 | 7 | 8 | 9 | 10 | myx 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | catkin 19 | 20 | message_generation 21 | std_msgs 22 | geometry_msgs 23 | actionlib 24 | actionlib_msgs 25 | 26 | message_runtime 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /rc_msgs/srv/ActionCmd.srv: -------------------------------------------------------------------------------- 1 | string action_name 2 | bool updateX_enable 3 | bool updateY_enable 4 | bool updateYaw_enable 5 | bool calibration_enable 6 | bool reset_enable 7 | float64 updateX_data 8 | float64 updateY_data 9 | float64 updateYaw_data 10 | --- 11 | bool is_success 12 | --------------------------------------------------------------------------------