├── .clang-format ├── .pre-commit-config.yaml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── modbus_hardware_interface │ ├── modbus_client.hpp │ ├── modbus_exceptions.hpp │ ├── modbus_hardware_interface.hpp │ └── visibility_control.h ├── launch └── local_mock_modbus_server.launch.xml ├── modbus_hardware_interface.xml ├── package.xml ├── src ├── modbus_hardware_interface.cpp └── modbus_mock_server_node.cpp └── test └── test_modbus_hardware_interface.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | ColumnLimit: 100 6 | AccessModifierOffset: -2 7 | AlignAfterOpenBracket: AlwaysBreak 8 | BreakBeforeBraces: Allman 9 | ConstructorInitializerIndentWidth: 0 10 | ContinuationIndentWidth: 2 11 | DerivePointerAlignment: false 12 | PointerAlignment: Middle 13 | ReflowComments: true 14 | IncludeBlocks: Preserve 15 | InsertBraces: true 16 | ... 17 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | 2 | # To use: 3 | # 4 | # pre-commit run -a 5 | # 6 | # Or: 7 | # 8 | # pre-commit install # (runs every time you commit in git) 9 | # 10 | # To update this file: 11 | # 12 | # pre-commit autoupdate 13 | # 14 | # See https://github.com/pre-commit/pre-commit 15 | 16 | repos: 17 | # Standard hooks 18 | - repo: https://github.com/pre-commit/pre-commit-hooks 19 | rev: v4.6.0 20 | hooks: 21 | - id: check-added-large-files 22 | - id: check-ast 23 | - id: check-case-conflict 24 | - id: check-docstring-first 25 | - id: check-merge-conflict 26 | - id: check-symlinks 27 | - id: check-xml 28 | - id: check-yaml 29 | - id: debug-statements 30 | - id: end-of-file-fixer 31 | - id: mixed-line-ending 32 | - id: trailing-whitespace 33 | exclude_types: [rst] 34 | - id: fix-byte-order-marker 35 | 36 | 37 | # Python hooks 38 | - repo: https://github.com/asottile/pyupgrade 39 | rev: v3.17.0 40 | hooks: 41 | - id: pyupgrade 42 | args: [--py36-plus] 43 | 44 | # PyDocStyle 45 | - repo: https://github.com/PyCQA/pydocstyle 46 | rev: 6.3.0 47 | hooks: 48 | - id: pydocstyle 49 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 50 | 51 | - repo: https://github.com/psf/black 52 | rev: 24.8.0 53 | hooks: 54 | - id: black 55 | args: ["--line-length=99"] 56 | 57 | - repo: https://github.com/pycqa/flake8 58 | rev: 7.1.1 59 | hooks: 60 | - id: flake8 61 | args: ["--extend-ignore=E501"] 62 | 63 | # CPP hooks 64 | - repo: https://github.com/pre-commit/mirrors-clang-format 65 | rev: v18.1.8 66 | hooks: 67 | - id: clang-format 68 | args: ['-fallback-style=none', '-i'] 69 | 70 | - repo: local 71 | hooks: 72 | - id: ament_cppcheck 73 | name: ament_cppcheck 74 | description: Static code analysis of C/C++ files. 75 | entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck 76 | language: system 77 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 78 | 79 | - repo: local 80 | hooks: 81 | - id: ament_cpplint 82 | name: ament_cpplint 83 | description: Static code analysis of C/C++ files. 84 | entry: ament_cpplint 85 | language: system 86 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 87 | args: ["--linelength=100", "--filter=-whitespace/newline"] 88 | 89 | # Cmake hooks 90 | - repo: local 91 | hooks: 92 | - id: ament_lint_cmake 93 | name: ament_lint_cmake 94 | description: Check format of CMakeLists.txt files. 95 | entry: ament_lint_cmake 96 | language: system 97 | files: CMakeLists\.txt$ 98 | 99 | # Copyright 100 | - repo: local 101 | hooks: 102 | - id: ament_copyright 103 | name: ament_copyright 104 | description: Check if copyright notice is available in all files. 105 | entry: ament_copyright 106 | language: system 107 | exclude: .*/conf\.py$ 108 | 109 | # Docs - RestructuredText hooks 110 | - repo: https://github.com/PyCQA/doc8 111 | rev: v1.1.1 112 | hooks: 113 | - id: doc8 114 | args: ['--max-line-length=100', '--ignore=D001'] 115 | exclude: CHANGELOG\.rst$ 116 | 117 | - repo: https://github.com/pre-commit/pygrep-hooks 118 | rev: v1.10.0 119 | hooks: 120 | - id: rst-backticks 121 | exclude: CHANGELOG\.rst$ 122 | - id: rst-directive-colons 123 | - id: rst-inline-touching-normal 124 | 125 | # Spellcheck in comments and docs 126 | # skipping of *.svg files is not working... 127 | - repo: https://github.com/codespell-project/codespell 128 | rev: v2.3.0 129 | hooks: 130 | - id: codespell 131 | args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] 132 | exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ 133 | 134 | - repo: https://github.com/python-jsonschema/check-jsonschema 135 | rev: 0.29.1 136 | hooks: 137 | - id: check-github-workflows 138 | args: ["--verbose"] 139 | - id: check-github-actions 140 | args: ["--verbose"] 141 | - id: check-dependabot 142 | args: ["--verbose"] 143 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(modbus_hardware_interface) 3 | 4 | if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") 5 | add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) 6 | endif() 7 | 8 | find_package(PkgConfig REQUIRED) # Find pkg-config module 9 | # Use pkg-config to get information about libmodbus 10 | pkg_check_modules(LIBMODBUS REQUIRED IMPORTED_TARGET libmodbus) 11 | 12 | # define ROS Package dependencies 13 | set(THIS_PACKAGE_INCLUDE_DEPENDS 14 | hardware_interface 15 | pluginlib 16 | rclcpp 17 | rclcpp_lifecycle 18 | rclpy 19 | ) 20 | 21 | # find dependencies 22 | find_package(ament_cmake REQUIRED) 23 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 24 | find_package(${Dependency} REQUIRED) 25 | endforeach() 26 | 27 | # modbus hardware interface 28 | add_library( 29 | modbus_hardware_interface 30 | SHARED 31 | src/modbus_hardware_interface.cpp 32 | ) 33 | target_include_directories( 34 | modbus_hardware_interface 35 | PUBLIC 36 | $ 37 | $ 38 | ) 39 | target_link_libraries(modbus_hardware_interface ${LIBMODBUS_LIBRARIES}) 40 | ament_target_dependencies(modbus_hardware_interface ${THIS_PACKAGE_INCLUDE_DEPENDS}) 41 | 42 | # modbus mock server 43 | add_executable(modbus_mock_server_node src/modbus_mock_server_node.cpp) 44 | target_link_libraries(modbus_mock_server_node ${LIBMODBUS_LIBRARIES}) 45 | ament_target_dependencies(modbus_mock_server_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) 46 | 47 | pluginlib_export_plugin_description_file( 48 | hardware_interface modbus_hardware_interface.xml) 49 | 50 | if(BUILD_TESTING) 51 | find_package(ament_cmake_gmock REQUIRED) 52 | find_package(ros2_control_test_assets REQUIRED) 53 | 54 | ament_add_gmock(test_modbus_hardware_interface test/test_modbus_hardware_interface.cpp) 55 | target_include_directories(test_modbus_hardware_interface PRIVATE include) 56 | ament_target_dependencies( 57 | test_modbus_hardware_interface 58 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 59 | ros2_control_test_assets 60 | ) 61 | 62 | endif() 63 | 64 | install( 65 | DIRECTORY include/ 66 | DESTINATION include/modbus_hardware_interface 67 | ) 68 | 69 | install( 70 | TARGETS modbus_hardware_interface 71 | EXPORT export_modbus_hardware_interface 72 | RUNTIME DESTINATION bin 73 | ARCHIVE DESTINATION lib 74 | LIBRARY DESTINATION lib 75 | ) 76 | 77 | install( 78 | TARGETS modbus_mock_server_node 79 | RUNTIME DESTINATION lib/${PROJECT_NAME} 80 | ) 81 | 82 | install(DIRECTORY launch/ 83 | DESTINATION share/${PROJECT_NAME} 84 | ) 85 | 86 | ament_export_targets(export_modbus_hardware_interface HAS_LIBRARY_TARGET) 87 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 88 | ament_package() 89 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Video explanation of its implemntation and how to use can be found [here](https://youtu.be/iEiZje8imeU?si=3jsBkv6SsXmHkQpW). 2 | # ModbusHardwareInterface 3 | If you not familiar with `ros2_control.xacro` check the [section at the end](#mini-urdf-and-xacro-introduction). 4 | ## Using ModbusHardwareInterface in ros2_control.xacro 5 | In this files we define the hardware components like the servo, linear actuator and proximity sensor. You can see there which component (plugin) is used and how the component is configured. 6 | 7 | Let's make an example: If we want to use the `ModbusHardwareInterface` in our project we can create a `my_example_hw.ros2_control.xacro`. In this file we define the used implementation with the `...` tag: 8 | 9 | 10 | 16 | 17 | 18 | modbus_hardware_interface/ModbusHardwareInterface 19 | ${modbus_server_ip} 20 | ${modbus_server_port} 21 | ${modbus_use_persistent_connection} 22 | 23 | 24 | 25 | ``` 26 | The plugin itself is defined in the `modbus_hardware_interface.xml` file. Additionally we set the ip and port for the modbus server there. 27 | 28 | Besides this the most important part of this files are that we define the provided *Command-/StateInterfaces* of the hardware component. 29 | 30 | ### Creation of Command-/StateInterfaces 31 | To define a command or state interface you have to supply following parameters for modbus:\ 32 | **CommandInterface:** 33 | ``` 34 | 35 | number_of_register*2 36 | 37 | how to convert from double to modbus int 38 | what modbus type 39 | for converting to modbus 40 | for converting to modbus 41 | 42 | ``` 43 | **StateInterface:** 44 | ``` 45 | 46 | umber_of_register*2 47 | number of bits to read/register size 48 | how to convert from modbus int to double 49 | what modbus type 50 | for converting from modbus 51 | or converting from modbus 52 | 53 | ``` 54 | #### Available xml parameters: 55 | The `ModbusHardwareInterface` supports the following parameters to configure how a StateInterface acquires its value from modbus or how a CommandInterface writes its value to modbus. 56 | ##### register (mandatory) 57 | Register number. Defines which register is requester for reading or writing. Check data sheet of your hardware for reference. 58 | ##### bits_to_read (mandatory) 59 | How many bits we read/write when accessing this modbus register. For registers this needs to be the register size and a multiple of `sizeof(uint_16t)`. If the register is not a multiple of 16, which is the underlying modbus int size, this could be problematic. For bit it says how many bits are read/written in one call. 60 | ##### conversion_fn 61 | For reading: This maps to a function which is used to convert from uint_16t (register) or uint_8t (bits) used in modbus to double used in ros2_control. You can check the modbus [library](https://libmodbus.org/reference/) to see which functions are available and the implementations in [modbus_client.hpp](https://github.com/StoglRobotics/modbus_hardware_interface/blob/master/include/modbus_hardware_interface/modbus_client.hpp). If you need some special conversion you have to implement.\ 62 | For writing: This maps to a function which is used to convert from double to uint_16t (register) or uint_8t (bits). 63 | ##### read_function/write_function (mandatory) 64 | read_function is for StatateIntefaces only, write_functions for CommandInterfaces only. \ 65 | We distinguish between reading a register or single bits. This is done by setting the *read_function/write_function*: 66 | * register -> read/write register 67 | * input_register -> used for read only registers 68 | * bits -> read/write bits 69 | * input_bits -> read only bits 70 | ##### factor and offset 71 | Used for converting from the special modbus datatype to double. This is different then a conversion from uint_8t/uint_16 to double. With the conversion_fn we just convert from uint_8tuint_16 to double. For example the consider the position StateInterface is given in ticks but we want to provide a position in m in our StateInterface. Therefore we have to convert the double we receive by simply converting from modbus to double further. Therefore we supply the options of giving a factor and offset-> factor*converted_uint_16+offset.\ 72 | The same is for the conversion from ros2_control double -> modbus uint_16 true. 73 | 74 | ## Using ModbusHardwareInterface in ros2_control.xacro 75 | If you need a custom HardwareInterface you first have to inherit from the `ModbusHardwareInterface`: 76 | ``` 77 | class MyServo : public modbus_hardware_interface::ModbusHardwareInterface 78 | ``` 79 | then override the functions you need some custom actions. Important! Don't forget to call the `ModbusHardwareInterface` functions in your implementations e.g. for: `on_init`, `read`, `write` like this 80 | 81 | ``` 82 | hardware_interface::CallbackReturn ServoHardwareInterface::on_init( 83 | const hardware_interface::HardwareInfo & info) 84 | { 85 | if (modbus_hardware_interface::ModbusHardwareInterface::on_init(info) != CallbackReturn::SUCCESS) 86 | { 87 | return CallbackReturn::ERROR; 88 | } 89 | ... 90 | } 91 | ``` 92 | ``` 93 | hardware_interface::return_type ServoHardwareInterface::read( 94 | const rclcpp::Time & time, const rclcpp::Duration & period) 95 | { 96 | auto ret = modbus_hardware_interface::ModbusHardwareInterface::read(time, period); 97 | if (ret != hardware_interface::return_type::OK) 98 | { 99 | return ret; 100 | } 101 | ..... 102 | } 103 | ``` 104 | If you do not call those the modbus specific parameters in `ros2_control.xacro` will not be parsed and the StateInterfaces will not be read or CommandInterfaces be written. 105 | 106 | # Mini URDF and Xacro introduction 107 | ## URDF 108 | URDF, Unified Robot Description Format is an XML format for representing a robot model. URDF is commonly used in Robot Operating System (ROS) tools such as rviz (Ros Visualization tool) and Gazebo simulator. The model consists of links and joints motion. 109 | ## [Xacro](https://github.com/ros/xacro/wiki) 110 | Xacro is an XML macro language and preprocessor that is typically used to simplify the generation of URDF files. However, it can be applied to any XML. Using parameterizable macros, re-occurring XML blocks can be created with a few commands only. 111 | ## ros2_control.xacro 112 | Check the [control docs](https://control.ros.org/rolling/doc/getting_started/getting_started.html#hardware-description-in-urdf) 113 | # References 114 | * [https://docs.ros.org/en/rolling/Tutorials/Intermediate/URDF/URDF-Main.html](https://docs.ros.org/en/rolling/Tutorials/Intermediate/URDF/URDF-Main.html) 115 | * [https://github.com/ros/xacro/wiki](https://github.com/ros/xacro/wiki) 116 | * [https://wiki.ros.org/xacro](https://wiki.ros.org/xacro) 117 | -------------------------------------------------------------------------------- /include/modbus_hardware_interface/modbus_client.hpp: -------------------------------------------------------------------------------- 1 | 2 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef MODBUS_HARDWARE_INTERFACE__MODBUS_CLIENT_HPP_ 17 | #define MODBUS_HARDWARE_INTERFACE__MODBUS_CLIENT_HPP_ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "modbus_hardware_interface/modbus_exceptions.hpp" 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | namespace modbus_hardware_interface 33 | { 34 | 35 | constexpr char REGISTER[] = "register"; 36 | constexpr char INPUT_REGISTER[] = "input_register"; 37 | constexpr char BITS[] = "bits"; 38 | constexpr char INPUT_BITS[] = "input_bits"; 39 | 40 | class ModbusInterfaceConfig 41 | { 42 | public: 43 | explicit ModbusInterfaceConfig( 44 | const int & register_address, const int & number_of_bits, const double & offset = 0.0, 45 | const double & factor = 1.0, const bool & register_mode = false, 46 | const int & modbus_register_length = 16) 47 | : register_address_(register_address), 48 | number_of_bits_(number_of_bits), 49 | offset_(offset), 50 | factor_(factor), 51 | register_mode_(register_mode), 52 | modbus_register_length_(modbus_register_length) 53 | { 54 | } 55 | 56 | int get_register_address() const { return register_address_; } 57 | 58 | int number_of_bits() const { return number_of_bits_; } 59 | 60 | int number_of_registers() const 61 | { 62 | // if we are in register mode then the bits to read must be a multiple of the 63 | // modbus_register_length_ 64 | assert(number_of_bits_ % modbus_register_length_ == 0); 65 | 66 | return number_of_bits_ / modbus_register_length_; 67 | } 68 | 69 | void register_mode(const bool & mode) { register_mode_ = mode; } 70 | 71 | bool register_mode() { return register_mode_; } 72 | 73 | protected: 74 | const int register_address_; // The address of the register 75 | const int number_of_bits_; // How many bits/registers to read 76 | double offset_; 77 | double factor_; 78 | bool register_mode_; 79 | const int modbus_register_length_; 80 | }; 81 | 82 | class ModbusInterfaceReadConfig : public ModbusInterfaceConfig 83 | { 84 | public: 85 | explicit ModbusInterfaceReadConfig( 86 | const int & register_address, const int & number_of_bits, const std::string & read_function, 87 | const std::string & conversion_fn, const double & offset = 0.0, const double & factor = 1.0) 88 | : ModbusInterfaceConfig(register_address, number_of_bits, offset, factor), 89 | modbus_uint_16_to_float_(nullptr), 90 | modbus_uint_8_to_float_(nullptr), 91 | read_function_(read_function) 92 | { 93 | if (this->read_function() == REGISTER || this->read_function() == INPUT_REGISTER) 94 | { 95 | this->register_mode(true); 96 | } 97 | else if (this->read_function() == BITS || this->read_function() == INPUT_BITS) 98 | { 99 | this->register_mode(false); 100 | } 101 | else 102 | { 103 | throw ModbusInvalidConfigException( 104 | "ModbusInterfaceReadConfig: Invalid read_function passed [" + read_function_ + 105 | "]. Allowed types are [" + std::string(REGISTER) + "], [" + std::string(INPUT_REGISTER) + 106 | "], [" + std::string(BITS) + "] or [" + std::string(INPUT_BITS) + "]."); 107 | } 108 | 109 | select_modbus_to_double_function(conversion_fn); 110 | } 111 | 112 | explicit ModbusInterfaceReadConfig( 113 | const int & register_address, const int & number_of_bits, const std::string & read_function, 114 | float (*custom_modbus_uint_16_to_float)(const uint16_t *), const double & offset = 0.0, 115 | const double & factor = 1.0) 116 | : ModbusInterfaceConfig(register_address, number_of_bits, offset, factor), 117 | modbus_uint_16_to_float_(custom_modbus_uint_16_to_float), 118 | modbus_uint_8_to_float_(nullptr), 119 | read_function_(read_function) 120 | { 121 | // check that mode fits to given readfunction signature 122 | if (this->read_function() != REGISTER && this->read_function() == INPUT_REGISTER) 123 | { 124 | throw ModbusInvalidConfigException( 125 | "ModbusInterfaceReadConfig: Custom function passed to ModbusReadConfig with signature for " 126 | "reading register but read_function set to: [" + 127 | read_function_ + "] were [" + std::string(REGISTER) + "] or [" + 128 | std::string(INPUT_REGISTER) + "] is expected."); 129 | } 130 | this->register_mode(true); 131 | } 132 | 133 | explicit ModbusInterfaceReadConfig( 134 | const int & register_address, const int & number_of_bits, const std::string & read_function, 135 | float (*custom_modbus_uint_8_to_float)(const uint8_t *), const double & offset = 0.0, 136 | const double & factor = 1.0) 137 | : ModbusInterfaceConfig(register_address, number_of_bits, offset, factor), 138 | modbus_uint_16_to_float_(nullptr), 139 | modbus_uint_8_to_float_(custom_modbus_uint_8_to_float), 140 | read_function_(read_function) 141 | { 142 | // check that read functions type (mode) fits to given readfunction signature 143 | if (this->read_function() != BITS && this->read_function() != INPUT_BITS) 144 | { 145 | throw ModbusInvalidConfigException( 146 | "ModbusInterfaceReadConfig: Custom function passed to ModbusReadConfig with signature for " 147 | "reading input bits but read_function set to: [" + 148 | read_function_ + "] were [" + std::string(BITS) + "] or [" + std::string(INPUT_BITS) + 149 | "] is expected."); 150 | } 151 | this->register_mode(false); 152 | } 153 | 154 | template 155 | double convert_from_modbus(const std::vector & modbus_value) 156 | { 157 | return static_cast(modbus_to_float(modbus_value)) * factor_ + offset_; 158 | } 159 | 160 | std::string read_function() const { return read_function_; } 161 | 162 | protected: 163 | float modbus_to_float(std::vector values) 164 | { 165 | THROW_ON_NULLPTR(modbus_uint_16_to_float_); 166 | return modbus_uint_16_to_float_(values.data()); 167 | } 168 | 169 | float modbus_to_float(std::vector values) 170 | { 171 | THROW_ON_NULLPTR(modbus_uint_8_to_float_); 172 | return modbus_uint_8_to_float_(values.data()); 173 | } 174 | 175 | void select_modbus_to_double_function(const std::string & conversion_fn) 176 | { 177 | if (register_mode()) 178 | { 179 | if (conversion_fn == "float_abcd") 180 | { 181 | modbus_uint_16_to_float_ = modbus_get_float_abcd; 182 | } 183 | else if (conversion_fn == "float_badc") 184 | { 185 | modbus_uint_16_to_float_ = modbus_get_float_badc; 186 | } 187 | else if (conversion_fn == "float_cdab") 188 | { 189 | modbus_uint_16_to_float_ = modbus_get_float_cdab; 190 | } 191 | else if (conversion_fn == "float_dcba") 192 | { 193 | modbus_uint_16_to_float_ = modbus_get_float_dcba; 194 | } 195 | else if (conversion_fn == "raw") 196 | { 197 | modbus_uint_16_to_float_ = nullptr; 198 | } 199 | else if (conversion_fn == "to_int_to_float") 200 | { 201 | modbus_uint_16_to_float_ = [](const uint16_t * values) -> float 202 | { 203 | // Dereference the pointer to access the uint16_t values 204 | uint16_t firstValue = *(values); 205 | uint16_t secondValue = *(values + 1); 206 | // Shift secondValue by 16 bits to occupy the higher 16 bits of the int32_t 207 | int32_t result = static_cast(secondValue) << 16; 208 | // Bitwise OR with firstValue to set the lower 16 bits 209 | result |= static_cast(firstValue); 210 | return static_cast(result); 211 | }; 212 | } 213 | else if (conversion_fn == "to_int_to_float_16") 214 | { 215 | modbus_uint_16_to_float_ = [](const uint16_t * values) -> float 216 | { 217 | // Dereference the pointer to access the uint16_t values 218 | uint16_t firstValue = *(values); 219 | // Shift secondValue by 16 bits to occupy the higher 16 bits of the int32_t 220 | int32_t result = 0x0; 221 | // Bitwise OR with firstValue to set the lower 16 bits 222 | result |= static_cast(firstValue); 223 | return static_cast(result); 224 | }; 225 | } 226 | else if (conversion_fn == "to_int_to_float_inv") 227 | { 228 | modbus_uint_16_to_float_ = [](const uint16_t * values) -> float 229 | { 230 | // Dereference the pointer to access the uint16_t values 231 | uint16_t firstValue = *(values); // Equivalent to values[0] 232 | uint16_t secondValue = *(values + 1); // Equivalent to values[1] 233 | 234 | // Shift firstValue by 16 bits to occupy the higher 16 bits of the int32_t 235 | int32_t result = static_cast(firstValue) << 16; 236 | // Bitwise OR with secondValue to set the lower 16 bits 237 | result |= static_cast(secondValue); 238 | return static_cast(result); 239 | }; 240 | } 241 | else 242 | { 243 | throw ModbusUnknownConversionFunctionException( 244 | "ModbusInterfaceReadConfig: Unknown modbus_uint_16_to_float_ function [" + conversion_fn + 245 | "] passed."); 246 | } 247 | } 248 | else 249 | { 250 | if (conversion_fn == "float") 251 | { 252 | modbus_uint_8_to_float_ = [](const uint8_t * value) -> float 253 | { return static_cast(*value); }; 254 | } 255 | else 256 | { 257 | throw ModbusUnknownConversionFunctionException( 258 | "ModbusInterfaceReadConfig: Unknown modbus_uint_8_to_float function [" + conversion_fn + 259 | "] passed."); 260 | } 261 | } 262 | } 263 | float (*modbus_uint_16_to_float_)(const uint16_t *); // [float_abcd, float_badc, float_dcba, ...] 264 | float (*modbus_uint_8_to_float_)(const uint8_t *); 265 | const std::string read_function_; // register, input_register, bits, input_bits 266 | }; 267 | 268 | class ModbusInterfaceWriteConfig : public ModbusInterfaceConfig 269 | { 270 | public: 271 | explicit ModbusInterfaceWriteConfig( 272 | const int & register_address, const int & number_of_number_of_bits, 273 | const std::string & write_function, const std::string & conversion_fn, 274 | const double & offset = 0.0, const double & factor = 1.0, 275 | const bool & write_this_interface = false) 276 | : ModbusInterfaceConfig(register_address, number_of_number_of_bits, offset, factor), 277 | modbus_float_to_uint_16_(nullptr), 278 | modbus_float_to_uint_8_(nullptr), 279 | write_function_(write_function), 280 | write_this_interface_(write_this_interface) 281 | { 282 | if (this->write_function() == REGISTER || this->write_function() == INPUT_REGISTER) 283 | { 284 | this->register_mode(true); 285 | } 286 | else if (this->write_function() == BITS || this->write_function() == INPUT_BITS) 287 | { 288 | this->register_mode(false); 289 | } 290 | else 291 | { 292 | throw ModbusInvalidConfigException( 293 | "ModbusInterfaceWriteConfig: Invalid write_function passed [" + write_function_ + 294 | "]. Allowed types are [" + std::string(REGISTER) + "], [" + std::string(INPUT_REGISTER) + 295 | "], [" + std::string(BITS) + "] or [" + std::string(INPUT_BITS) + "]."); 296 | } 297 | select_double_to_modbus_function(conversion_fn); 298 | } 299 | 300 | explicit ModbusInterfaceWriteConfig( 301 | const int & register_address, const int & number_of_number_of_bits, 302 | const std::string & write_function, 303 | std::vector (*custom_modbus_float_to_uint_16)(const float &), 304 | const double & offset = 0.0, const double & factor = 1.0, 305 | const bool & write_this_interface = false) 306 | : ModbusInterfaceConfig(register_address, number_of_number_of_bits, offset, factor), 307 | modbus_float_to_uint_16_(custom_modbus_float_to_uint_16), 308 | modbus_float_to_uint_8_(nullptr), 309 | write_function_(write_function), 310 | write_this_interface_(write_this_interface) 311 | { 312 | if (this->write_function() != REGISTER && this->write_function() != INPUT_REGISTER) 313 | { 314 | throw ModbusInvalidConfigException( 315 | "ModbusInterfaceWriteConfig: Custom function passed to ModbusInterfaceWriteConfig with " 316 | "signature for writing register but write_function_ set to: [" + 317 | write_function_ + "] were [" + std::string(REGISTER) + "] or [" + 318 | std::string(INPUT_REGISTER) + "] is expected."); 319 | } 320 | this->register_mode(true); 321 | } 322 | 323 | explicit ModbusInterfaceWriteConfig( 324 | const int & register_address, const int & number_of_number_of_bits, 325 | const std::string & write_function, 326 | std::vector (*custom_modbus_float_to_uint_8)(const float &), 327 | const double & offset = 0.0, const double & factor = 1.0, 328 | const bool & write_this_interface = false) 329 | : ModbusInterfaceConfig(register_address, number_of_number_of_bits, offset, factor), 330 | modbus_float_to_uint_16_(nullptr), 331 | modbus_float_to_uint_8_(custom_modbus_float_to_uint_8), 332 | write_function_(write_function), 333 | write_this_interface_(write_this_interface) 334 | { 335 | if (this->write_function() != BITS && this->write_function() != INPUT_BITS) 336 | { 337 | throw ModbusInvalidConfigException( 338 | "ModbusInterfaceWriteConfig: Custom function passed to ModbusInterfaceWriteConfig with " 339 | "signature for writing bits but write_function_ set to: [" + 340 | write_function_ + "] were were [" + std::string(BITS) + "] or [" + std::string(INPUT_BITS) + 341 | "] is expected."); 342 | } 343 | this->register_mode(false); 344 | } 345 | 346 | std::vector double_to_modbus_16(const double & value) 347 | { 348 | THROW_ON_NULLPTR(modbus_float_to_uint_16_); 349 | return modbus_float_to_uint_16_(static_cast(convert(value))); 350 | } 351 | 352 | std::vector double_to_modbus_8(const double & value) 353 | { 354 | THROW_ON_NULLPTR(modbus_float_to_uint_8_); 355 | return modbus_float_to_uint_8_(static_cast(convert(value))); 356 | } 357 | 358 | std::string write_function() const { return write_function_; } 359 | 360 | void write_this_interface(const bool & write_this_interface) 361 | { 362 | write_this_interface_ = write_this_interface; 363 | } 364 | 365 | bool write_this_interface() const { return write_this_interface_; } 366 | 367 | protected: 368 | double convert(const double & value) { return value * factor_ + offset_; } 369 | 370 | void select_double_to_modbus_function(const std::string & conversion_fn) 371 | { 372 | if (register_mode()) 373 | { 374 | if (conversion_fn == "float_abcd") 375 | { 376 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 377 | { 378 | std::vector data(2); 379 | modbus_set_float_abcd(value, data.data()); 380 | return data; 381 | }; 382 | } 383 | else if (conversion_fn == "float_badc") 384 | { 385 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 386 | { 387 | std::vector data(2); 388 | modbus_set_float_badc(value, data.data()); 389 | return data; 390 | }; 391 | } 392 | else if (conversion_fn == "float_cdab") 393 | { 394 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 395 | { 396 | std::vector data(2); 397 | modbus_set_float_cdab(value, data.data()); 398 | return data; 399 | }; 400 | } 401 | else if (conversion_fn == "raw") 402 | { 403 | modbus_float_to_uint_16_ = nullptr; 404 | } 405 | else if (conversion_fn == "float_dcba") 406 | { 407 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 408 | { 409 | std::vector data(2); 410 | modbus_set_float_dcba(value, data.data()); 411 | return data; 412 | }; 413 | } 414 | else if (conversion_fn == "to_int_to_modbus") 415 | { 416 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 417 | { 418 | int32_t int_value = static_cast(std::round(value)); 419 | std::vector data(2); 420 | // Extract the lower 16 bits (first uint16_t value) 421 | data[0] = static_cast(int_value & 0xFFFF); 422 | // Extract the higher 16 bits (second uint16_t value) 423 | data[1] = static_cast(int_value >> 16); 424 | return data; 425 | }; 426 | } 427 | else if (conversion_fn == "to_int_to_modbus_inv") 428 | { 429 | modbus_float_to_uint_16_ = [](const float & value) -> std::vector 430 | { 431 | int32_t int_value = static_cast(std::round(value)); 432 | std::vector data(2); 433 | // Extract the higher 16 bits (second uint16_t value) 434 | data[0] = static_cast(int_value >> 16); 435 | // Extract the lower 16 bits (first uint16_t value) 436 | data[1] = static_cast(int_value & 0xFFFF); 437 | return data; 438 | }; 439 | } 440 | else 441 | { 442 | throw ModbusUnknownConversionFunctionException( 443 | "ModbusInterfaceWriteConfig: Unknown modbus_uint_16_to_float function [" + conversion_fn + 444 | "] passed."); 445 | } 446 | } 447 | else 448 | { 449 | if (conversion_fn == "float") 450 | { 451 | modbus_float_to_uint_8_ = [](const float & value) -> std::vector 452 | { 453 | std::vector data(1); 454 | // TODO(Manual) Maybe clamp, round and check that its zero/one? 455 | data.push_back(static_cast(value)); 456 | return data; 457 | }; 458 | } 459 | else 460 | { 461 | throw ModbusUnknownConversionFunctionException( 462 | "ModbusInterfaceWriteConfig: Unknown modbus_8_to_float function [" + conversion_fn + 463 | "] passed."); 464 | } 465 | } 466 | } 467 | 468 | std::vector (*modbus_float_to_uint_16_)(const float &); 469 | std::vector (*modbus_float_to_uint_8_)(const float &); 470 | const std::string write_function_; // register, input_register, bits, input_bits 471 | bool write_this_interface_; 472 | }; 473 | 474 | class ModbusClient 475 | { 476 | public: 477 | explicit ModbusClient( 478 | const std::string & ip_address, const int & port, const bool & persistent_connection = true, 479 | modbus_t * modbus_connection = nullptr) 480 | : ip_address_(ip_address), 481 | port_(port), 482 | persistent_connection_(persistent_connection), 483 | modbus_connection_(modbus_connection) 484 | { 485 | RCLCPP_INFO( 486 | rclcpp::get_logger("ModbusConnection"), 487 | "Create connection on %s:%i with persistent connection <%s>", ip_address_.c_str(), port_, 488 | persistent_connection_ ? "true" : "false"); 489 | } 490 | 491 | ~ModbusClient() { disconnect(); } 492 | 493 | bool connected() const { return modbus_connection_ != nullptr; } 494 | 495 | inline operator bool() const { return connected(); } 496 | 497 | bool is_persistent_connection() const { return persistent_connection_; } 498 | 499 | bool connect() 500 | { 501 | try 502 | { 503 | if (!modbus_connection_) 504 | { 505 | modbus_connection_ = modbus_new_tcp(ip_address_.c_str(), port_); 506 | } 507 | if (modbus_connection_ == NULL) 508 | { 509 | RCLCPP_ERROR( 510 | rclcpp::get_logger("ModbusConnection"), "Unable to create the libmodbus context"); 511 | disconnect(); 512 | return false; 513 | } 514 | 515 | if (modbus_connect(modbus_connection_) == -1) 516 | { 517 | RCLCPP_ERROR( 518 | rclcpp::get_logger("ModbusConnection"), "Connection failed: %s", modbus_strerror(errno)); 519 | disconnect(); 520 | return false; 521 | } 522 | 523 | RCLCPP_INFO( 524 | rclcpp::get_logger("ModbusConnection"), "Connected to Modbus server at %s:%d", 525 | ip_address_.c_str(), port_); 526 | } 527 | catch (const std::exception & e) 528 | { 529 | RCLCPP_ERROR( 530 | rclcpp::get_logger("ModbusConnection"), "Exception in connectToModbusServer: %s", e.what()); 531 | disconnect(); 532 | return false; 533 | } 534 | return true; 535 | } 536 | 537 | void disconnect() 538 | { 539 | if (modbus_connection_ != nullptr) 540 | { 541 | modbus_close(modbus_connection_); 542 | modbus_free(modbus_connection_); 543 | modbus_connection_ = nullptr; 544 | } 545 | } 546 | 547 | std::vector read_raw_register(ModbusInterfaceReadConfig & config) 548 | { 549 | const std::string read_fn = config.read_function(); 550 | if (read_fn == REGISTER) 551 | { 552 | return read_register(config.get_register_address(), config.number_of_registers()); 553 | } 554 | else if (read_fn == INPUT_REGISTER) 555 | { 556 | return read_input_register(config.get_register_address(), config.number_of_registers()); 557 | } 558 | else 559 | { 560 | throw std::runtime_error( 561 | "ModbusClient: Error while reading raw register. The given read function [" + read_fn + 562 | "] is not defined for registers"); 563 | } 564 | } 565 | 566 | double read(ModbusInterfaceReadConfig & config) 567 | { 568 | const std::string read_fn = config.read_function(); 569 | if (read_fn == REGISTER) 570 | { 571 | return config.convert_from_modbus( 572 | read_register(config.get_register_address(), config.number_of_registers())); 573 | } 574 | else if (read_fn == INPUT_REGISTER) 575 | { 576 | return config.convert_from_modbus( 577 | read_input_register(config.get_register_address(), config.number_of_registers())); 578 | } 579 | else if (read_fn == BITS) 580 | { 581 | return config.convert_from_modbus( 582 | read_bits(config.get_register_address(), config.number_of_bits())); 583 | } 584 | else if (read_fn == INPUT_BITS) 585 | { 586 | return config.convert_from_modbus( 587 | read_input_bits(config.get_register_address(), config.number_of_bits())); 588 | } 589 | else 590 | { 591 | throw std::runtime_error( 592 | "ModbusClient: Error while reading. The given read function [" + read_fn + 593 | "] is not defined."); 594 | } 595 | } 596 | 597 | // TODO(Manual) where should we check if there is not a value change happening? 598 | // Client or driver? 599 | void write(ModbusInterfaceWriteConfig config, const double & value) 600 | { 601 | const std::string write_fn = config.write_function(); 602 | if (write_fn == REGISTER) 603 | { 604 | write_register(config.get_register_address(), config.double_to_modbus_16(value)); 605 | } 606 | else if (write_fn == BITS) 607 | { 608 | write_bits(config.get_register_address(), config.double_to_modbus_8(value)); 609 | } 610 | else 611 | { 612 | throw std::runtime_error( 613 | "ModbusClient: Error while writing. The given write function [" + write_fn + 614 | "] is not defined."); 615 | } 616 | } 617 | 618 | private: 619 | std::vector read_bits(const int & register_address, const int & number_of_bits = 1) 620 | { 621 | std::vector bits(number_of_bits); 622 | int rc = modbus_read_bits(modbus_connection_, register_address, number_of_bits, bits.data()); 623 | if (rc == -1) 624 | { 625 | std::string error_msg = "ModbusConnection: Failed to read bits in register [" + 626 | std::to_string(register_address) + "]"; 627 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 628 | 629 | throw ModbusReadException(error_msg); 630 | // just to have right default in case exception is removed 631 | return std::vector(); 632 | } 633 | return bits; 634 | } 635 | 636 | std::vector read_input_bits(const int & register_address, const int & number_of_bits = 1) 637 | { 638 | std::vector bits(number_of_bits); 639 | int rc = 640 | modbus_read_input_bits(modbus_connection_, register_address, number_of_bits, bits.data()); 641 | if (rc == -1) 642 | { 643 | std::string error_msg = "ModbusConnection: Failed to read bits in register [" + 644 | std::to_string(register_address) + "]"; 645 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 646 | 647 | throw ModbusReadException(error_msg); 648 | // just to have right default in case exception is removed 649 | return std::vector(); 650 | } 651 | return bits; 652 | } 653 | 654 | std::vector read_register( 655 | const int & register_address, const int & number_of_registers = 2) 656 | { 657 | std::vector reg_dest(number_of_registers); 658 | int rc = modbus_read_registers( 659 | modbus_connection_, register_address, number_of_registers, reg_dest.data()); 660 | if (rc == -1) 661 | { 662 | std::string error_msg = 663 | "ModbusConnection: Failed to read register [" + std::to_string(register_address) + "]"; 664 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 665 | 666 | throw ModbusReadException(error_msg); 667 | // just to have right default in case exception is removed 668 | return std::vector(); 669 | } 670 | return reg_dest; 671 | } 672 | 673 | std::vector read_input_register( 674 | const int & register_address, const int & number_of_registers = 2) 675 | { 676 | std::vector reg_dest(number_of_registers); 677 | int rc = modbus_read_input_registers( 678 | modbus_connection_, register_address, number_of_registers, reg_dest.data()); 679 | if (rc == -1) 680 | { 681 | std::string error_msg = "ModbusConnection: Failed to read input register [" + 682 | std::to_string(register_address) + "]"; 683 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 684 | 685 | throw ModbusReadException(error_msg); 686 | // just to have right default in case exception is removed 687 | return std::vector(); 688 | } 689 | return reg_dest; 690 | } 691 | 692 | int write_bits(const int & register_address, const std::vector & bits) 693 | { 694 | // static cast should not pose a problem here since its very unlikely that we want to read more 695 | // than 2^31 bits 696 | int wc = modbus_write_bits( 697 | modbus_connection_, register_address, static_cast(bits.size()), bits.data()); 698 | if (wc == -1) 699 | { 700 | std::string error_msg = "ModbusConnection: Failed to write bits in register [" + 701 | std::to_string(register_address) + "]"; 702 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 703 | 704 | throw ModbusWriteException(error_msg); 705 | // just to have right default in case exception is removed 706 | return wc; 707 | } 708 | return wc; 709 | } 710 | 711 | int write_register(const int & register_address, std::vector values) 712 | { 713 | int wc = modbus_write_registers( 714 | modbus_connection_, register_address, static_cast(values.size()), values.data()); 715 | if (wc == -1) 716 | { 717 | std::string error_msg = 718 | "ModbusConnection: Failed to write register [" + std::to_string(register_address) + "]"; 719 | RCLCPP_WARN(rclcpp::get_logger("ModbusConnection"), error_msg.c_str()); 720 | 721 | throw ModbusWriteException(error_msg); 722 | // just to have right default in case exception is removed 723 | return wc; 724 | } 725 | return wc; 726 | } 727 | 728 | // Private members 729 | std::string ip_address_; 730 | int port_; 731 | bool persistent_connection_; 732 | modbus_t * modbus_connection_; 733 | }; 734 | 735 | } // namespace modbus_hardware_interface 736 | 737 | #endif // MODBUS_HARDWARE_INTERFACE__MODBUS_CLIENT_HPP_ 738 | -------------------------------------------------------------------------------- /include/modbus_hardware_interface/modbus_exceptions.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MODBUS_HARDWARE_INTERFACE__MODBUS_EXCEPTIONS_HPP_ 16 | #define MODBUS_HARDWARE_INTERFACE__MODBUS_EXCEPTIONS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace modbus_hardware_interface 22 | { 23 | 24 | class ModbusReadException : public std::runtime_error 25 | { 26 | public: 27 | explicit ModbusReadException(const std::string & message) : std::runtime_error(message) {} 28 | 29 | virtual ~ModbusReadException() noexcept {} 30 | }; 31 | 32 | class ModbusWriteException : public std::runtime_error 33 | { 34 | public: 35 | explicit ModbusWriteException(const std::string & message) : std::runtime_error(message) {} 36 | 37 | virtual ~ModbusWriteException() noexcept {} 38 | }; 39 | 40 | class ModbusConnectionException : public std::runtime_error 41 | { 42 | public: 43 | explicit ModbusConnectionException(const std::string & message) : std::runtime_error(message) {} 44 | 45 | virtual ~ModbusConnectionException() noexcept {} 46 | }; 47 | 48 | class ModbusInvalidConfigException : public std::runtime_error 49 | { 50 | public: 51 | explicit ModbusInvalidConfigException(const std::string & message) : std::runtime_error(message) 52 | { 53 | } 54 | 55 | virtual ~ModbusInvalidConfigException() noexcept {} 56 | }; 57 | 58 | class ModbusUnknownConversionFunctionException : public std::runtime_error 59 | { 60 | public: 61 | explicit ModbusUnknownConversionFunctionException(const std::string & message) 62 | : std::runtime_error(message) 63 | { 64 | } 65 | 66 | virtual ~ModbusUnknownConversionFunctionException() noexcept {} 67 | }; 68 | 69 | } // namespace modbus_hardware_interface 70 | 71 | #endif // MODBUS_HARDWARE_INTERFACE__MODBUS_EXCEPTIONS_HPP_ 72 | -------------------------------------------------------------------------------- /include/modbus_hardware_interface/modbus_hardware_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MODBUS_HARDWARE_INTERFACE__MODBUS_HARDWARE_INTERFACE_HPP_ 16 | #define MODBUS_HARDWARE_INTERFACE__MODBUS_HARDWARE_INTERFACE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "hardware_interface/handle.hpp" 25 | #include "hardware_interface/hardware_info.hpp" 26 | #include "hardware_interface/system_interface.hpp" 27 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 28 | #include "modbus_hardware_interface/modbus_client.hpp" 29 | #include "modbus_hardware_interface/visibility_control.h" 30 | #include "rclcpp/macros.hpp" 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "rclcpp_lifecycle/state.hpp" 33 | 34 | namespace modbus_hardware_interface 35 | { 36 | // Default values for modbus 37 | static const bool USE_PERSISTENT_CONNECTION_DEFAULT = true; 38 | static const int NUMBER_OF_BITS_TO_READ_DEFAULT = 32; 39 | static const char CONVERSION_FN_DEFAULT[] = "float_dcba"; 40 | static const char READ_FUNCTION_DEFAULT[] = "register"; 41 | static const char WRITE_FUNCTION_DEFAULT[] = "register"; 42 | static const double NO_CMD = std::numeric_limits::quiet_NaN(); 43 | 44 | class ModbusHardwareInterface : public hardware_interface::SystemInterface 45 | { 46 | public: 47 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 48 | hardware_interface::CallbackReturn on_init( 49 | const hardware_interface::HardwareInfo & info) override; 50 | 51 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 52 | hardware_interface::CallbackReturn on_configure( 53 | const rclcpp_lifecycle::State & previous_state) override; 54 | 55 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 56 | std::vector export_state_interfaces() override; 57 | 58 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 59 | std::vector export_command_interfaces() override; 60 | 61 | // TODO(destogl): remove this method from here 62 | 63 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 64 | hardware_interface::CallbackReturn on_activate( 65 | const rclcpp_lifecycle::State & previous_state) override; 66 | 67 | // TODO(destogl): remove this method from here 68 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 69 | hardware_interface::CallbackReturn on_deactivate( 70 | const rclcpp_lifecycle::State & previous_state) override; 71 | 72 | // TODO(destogl): remove this method from here 73 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 74 | hardware_interface::return_type prepare_command_mode_switch( 75 | const std::vector & start_interfaces, 76 | const std::vector & stop_interfaces) override; 77 | 78 | // TODO(destogl): remove this method from here 79 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 80 | hardware_interface::return_type perform_command_mode_switch( 81 | const std::vector & start_interfaces, 82 | const std::vector & stop_interfaces) override; 83 | 84 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 85 | hardware_interface::return_type read( 86 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 87 | 88 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 89 | hardware_interface::return_type write( 90 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 91 | 92 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 93 | hardware_interface::CallbackReturn on_cleanup( 94 | const rclcpp_lifecycle::State & previous_state) override; 95 | 96 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 97 | hardware_interface::CallbackReturn on_error( 98 | const rclcpp_lifecycle::State & previous_state) override; 99 | 100 | protected: 101 | /** 102 | * For a given interface the parameters from the IntefaceInfo like register number, bits to read, 103 | * ... are parsed and the corresponding ModbusInterfaceConfig is created. 104 | * 105 | * \param[in] interface info about Command/StateInteface containing the parameters 106 | * \param[in] access_function 107 | * 108 | * \return For StateInterface a ModbusInterfaceReadConfig is returned, for CommandInterface a 109 | * ModbusInterfaceWriteConfig 110 | */ 111 | template 112 | T create_config( 113 | hardware_interface::InterfaceInfo & interface, const std::string & access_function); 114 | 115 | /** 116 | * Check if the connection to the modbus server is established. If not try to initialize it. 117 | * 118 | * \return returns true if connected or connection to modbus server could be established. 119 | * Otherwise false is returned 120 | */ 121 | bool connection_established() const; 122 | 123 | /** 124 | * If the modbus clients is set to non_persistent (which means we don't want to hold the 125 | * connection if we don't want to send/receive data) the connection is disconnected after 126 | * reading/writing 127 | */ 128 | void disconnect_non_persistent_connection() const; 129 | 130 | std::unordered_map state_interface_to_config_; 131 | std::unordered_map command_interface_to_config_; 132 | std::unordered_map state_interface_to_states_; 133 | std::unordered_map command_interface_to_commands_; 134 | std::unique_ptr client_; 135 | }; 136 | 137 | } // namespace modbus_hardware_interface 138 | 139 | #endif // MODBUS_HARDWARE_INTERFACE__MODBUS_HARDWARE_INTERFACE_HPP_ 140 | -------------------------------------------------------------------------------- /include/modbus_hardware_interface/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MODBUS_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 16 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 17 | 18 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 19 | // https://gcc.gnu.org/wiki/Visibility 20 | 21 | #if defined _WIN32 || defined __CYGWIN__ 22 | #ifdef __GNUC__ 23 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_EXPORT __attribute__((dllexport)) 24 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_IMPORT __attribute__((dllimport)) 25 | #else 26 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_EXPORT __declspec(dllexport) 27 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_IMPORT __declspec(dllimport) 28 | #endif 29 | #ifdef MODBUS_HARDWARE_INTERFACE__VISIBILITY_BUILDING_DLL 30 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC MODBUS_HARDWARE_INTERFACE__VISIBILITY_EXPORT 31 | #else 32 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC MODBUS_HARDWARE_INTERFACE__VISIBILITY_IMPORT 33 | #endif 34 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC_TYPE \ 35 | MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 36 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_LOCAL 37 | #else 38 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_EXPORT __attribute__((visibility("default"))) 39 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_IMPORT 40 | #if __GNUC__ >= 4 41 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC __attribute__((visibility("default"))) 42 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) 43 | #else 44 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC 45 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_LOCAL 46 | #endif 47 | #define MODBUS_HARDWARE_INTERFACE__VISIBILITY_PUBLIC_TYPE 48 | #endif 49 | 50 | #endif // MODBUS_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 51 | -------------------------------------------------------------------------------- /launch/local_mock_modbus_server.launch.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 20 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /modbus_hardware_interface.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | ros2_control hardware interface for the servo motor controlled via the modbus protocol. 8 | 9 | 10 | 11 | s 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | modbus_hardware_interface 5 | 0.0.0 6 | Generic hardware interface for Modbus Devices usage in ros2_control. 7 | Daniel Azanov 8 | Manual Muth 9 | Dr. Denis Stogl 10 | Apache-2.0 11 | 12 | ament_cmake 13 | pkg-config 14 | 15 | hardware_interface 16 | libmodbus-dev 17 | pluginlib 18 | rclcpp 19 | rclcpp_lifecycle 20 | 21 | ament_cmake_gmock 22 | ros2_control_test_assets 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/modbus_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "modbus_hardware_interface/modbus_hardware_interface.hpp" 16 | #include "modbus_hardware_interface/modbus_exceptions.hpp" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "hardware_interface/lexical_casts.hpp" 23 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 24 | 25 | namespace modbus_hardware_interface 26 | { 27 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_init( 28 | const hardware_interface::HardwareInfo & info) 29 | { 30 | if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) 31 | { 32 | return CallbackReturn::ERROR; 33 | } 34 | 35 | // TODO(Manual) Check if ip is valid? 36 | // Get IP of server 37 | std::string modbus_server_ip = info_.hardware_parameters["modbus_server_ip"]; 38 | if (modbus_server_ip.empty()) 39 | { 40 | RCLCPP_ERROR_STREAM( 41 | rclcpp::get_logger("ModbusHardwareInterface"), 42 | "ModbusHardwareInterface server ip for this hardware is empty. Need to set a ip address of " 43 | "the modbus server."); 44 | return CallbackReturn::ERROR; 45 | } 46 | 47 | // Get port of server 48 | // catch and return with error. Print to provide some additional information for user where it 49 | // failed since we don't provide default value for the port and crash in case the port is 50 | // unset/invalid 51 | int modbus_server_port; 52 | try 53 | { 54 | modbus_server_port = std::stoi(info_.hardware_parameters["modbus_server_port"]); 55 | } 56 | catch (const std::invalid_argument & e) 57 | { 58 | RCLCPP_ERROR_STREAM( 59 | rclcpp::get_logger("ModbusHardwareInterface"), 60 | "ModbusHardwareInterface: Error while getting port for Hardware [" + info.name + "] " + 61 | e.what()); 62 | return CallbackReturn::ERROR; 63 | } 64 | catch (const std::out_of_range & e) 65 | { 66 | RCLCPP_ERROR_STREAM( 67 | rclcpp::get_logger("ModbusHardwareInterface"), 68 | "ModbusHardwareInterface: Error while getting port for Hardware[" + info.name + "] " + 69 | e.what()); 70 | return CallbackReturn::ERROR; 71 | } 72 | 73 | // Get connection type (persistent/not persistent) of the modbus client 74 | // persistent -> we establish connection once and try to hold it the entire time 75 | // non persistent -> we establish connection before each read/write and then disconnect 76 | bool use_persistent_connection; 77 | std::string use_persistent_connection_str = 78 | info_.hardware_parameters["use_persistent_connection"]; 79 | if (use_persistent_connection_str.empty()) 80 | { 81 | RCLCPP_WARN_STREAM( 82 | rclcpp::get_logger("ModbusHardwareInterface"), 83 | "use_persistent_connection is neither set to true or false for Hardware[" + info.name + 84 | "]. Use '" 85 | << USE_PERSISTENT_CONNECTION_DEFAULT << "' as default."); 86 | use_persistent_connection = USE_PERSISTENT_CONNECTION_DEFAULT; 87 | } 88 | else 89 | { 90 | use_persistent_connection = hardware_interface::parse_bool(use_persistent_connection_str); 91 | } 92 | 93 | client_ = 94 | std::make_unique(modbus_server_ip, modbus_server_port, use_persistent_connection); 95 | 96 | // initialize the configurations for each command and state/interface of each joint 97 | for (hardware_interface::ComponentInfo & joint : info_.joints) 98 | { 99 | // get parameters for each StateInterface 100 | for (auto & state_interface : joint.state_interfaces) 101 | { 102 | std::string state_interface_name = joint.name + "/" + state_interface.name; 103 | // initialize with no command received 104 | state_interface_to_states_[state_interface_name] = NO_CMD; 105 | // Not used with modbus 106 | if (state_interface.parameters["register"].empty()) 107 | { 108 | continue; 109 | } 110 | // read_function, set to default if not provided 111 | std::string read_function = state_interface.parameters["read_function"]; 112 | if (read_function.empty()) 113 | { 114 | RCLCPP_WARN_STREAM( 115 | rclcpp::get_logger("ModbusHardwareInterface"), 116 | "read_function is empty for interface[" + state_interface_name + "]. Use '" 117 | << READ_FUNCTION_DEFAULT << "' as default."); 118 | read_function = READ_FUNCTION_DEFAULT; 119 | } 120 | state_interface_to_config_.insert(std::make_pair( 121 | state_interface_name, 122 | create_config(state_interface, read_function))); 123 | } 124 | 125 | // get parameters for each CommandInterface 126 | for (auto & command_interface : joint.command_interfaces) 127 | { 128 | std::string command_interface_name = joint.name + "/" + command_interface.name; 129 | 130 | // set initial to no command 131 | command_interface_to_commands_[command_interface_name] = NO_CMD; 132 | // Not used with modbus 133 | if (command_interface.parameters["register"].empty()) 134 | { 135 | continue; 136 | } 137 | // write_function, set to default if not provided 138 | std::string write_function = command_interface.parameters["write_function"]; 139 | if (write_function.empty()) 140 | { 141 | RCLCPP_WARN_STREAM( 142 | rclcpp::get_logger("ModbusHardwareInterface"), 143 | "write_function is empty for interface[" + command_interface_name + "]. Use '" 144 | << WRITE_FUNCTION_DEFAULT << "' as default."); 145 | write_function = WRITE_FUNCTION_DEFAULT; 146 | } 147 | command_interface_to_config_.insert(std::make_pair( 148 | command_interface_name, 149 | create_config(command_interface, write_function))); 150 | } 151 | } 152 | RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ModbusHardwareInterface"), "Finished `on_init`!"); 153 | 154 | return CallbackReturn::SUCCESS; 155 | } 156 | 157 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_configure( 158 | const rclcpp_lifecycle::State & /*previous_state*/) 159 | { 160 | if (client_->is_persistent_connection()) 161 | { 162 | if (!client_->connect()) 163 | { 164 | RCLCPP_ERROR_STREAM( 165 | rclcpp::get_logger("ModbusHardwareInterface"), 166 | "could not establish connection to modbus server."); 167 | return CallbackReturn::ERROR; 168 | } 169 | } 170 | return CallbackReturn::SUCCESS; 171 | } 172 | 173 | std::vector ModbusHardwareInterface::export_state_interfaces() 174 | { 175 | std::vector state_interfaces; 176 | for (const auto & joint : info_.joints) 177 | { 178 | for (const auto & state_interface : joint.state_interfaces) 179 | { 180 | std::string state_interface_name = joint.name + "/" + state_interface.name; 181 | state_interfaces.emplace_back(hardware_interface::StateInterface( 182 | joint.name, state_interface.name, &state_interface_to_states_[state_interface_name])); 183 | } 184 | } 185 | 186 | // sensors 187 | for (const auto & sensor : info_.sensors) 188 | { 189 | for (const auto & state_interface : sensor.state_interfaces) 190 | { 191 | std::string state_interface_name = sensor.name + "/" + state_interface.name; 192 | state_interfaces.emplace_back(hardware_interface::StateInterface( 193 | sensor.name, state_interface.name, &state_interface_to_states_[state_interface_name])); 194 | } 195 | } 196 | 197 | return state_interfaces; 198 | } 199 | 200 | std::vector 201 | ModbusHardwareInterface::export_command_interfaces() 202 | { 203 | std::vector command_interfaces; 204 | for (const auto & joint : info_.joints) 205 | { 206 | for (const auto & command_interface : joint.command_interfaces) 207 | { 208 | std::string command_interface_name = joint.name + "/" + command_interface.name; 209 | command_interfaces.emplace_back(hardware_interface::CommandInterface( 210 | joint.name, command_interface.name, 211 | &command_interface_to_commands_[command_interface_name])); 212 | } 213 | } 214 | 215 | return command_interfaces; 216 | } 217 | 218 | // TODO(destogl): remove this method from here 219 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_activate( 220 | const rclcpp_lifecycle::State & /*previous_state*/) 221 | { 222 | RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ModbusHardwareInterface"), "activate"); 223 | return CallbackReturn::SUCCESS; 224 | } 225 | 226 | // TODO(destogl): remove this method from here 227 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_deactivate( 228 | const rclcpp_lifecycle::State & /*previous_state*/) 229 | { 230 | RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ModbusHardwareInterface"), "deactivate"); 231 | return CallbackReturn::SUCCESS; 232 | } 233 | 234 | // TODO(destogl): remove this method from here 235 | hardware_interface::return_type ModbusHardwareInterface::prepare_command_mode_switch( 236 | const std::vector & /*start_interfaces*/, 237 | const std::vector & /*stop_interfaces*/) 238 | { 239 | return hardware_interface::return_type::OK; 240 | } 241 | 242 | // TODO(destogl): remove this method from here 243 | hardware_interface::return_type ModbusHardwareInterface::perform_command_mode_switch( 244 | const std::vector & /*start_interfaces*/, 245 | const std::vector & /*stop_interfaces*/) 246 | { 247 | return hardware_interface::return_type::OK; 248 | } 249 | 250 | hardware_interface::return_type ModbusHardwareInterface::read( 251 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 252 | { 253 | if (!connection_established()) 254 | { 255 | return hardware_interface::return_type::ERROR; 256 | } 257 | for (auto & [name, config] : state_interface_to_config_) 258 | { 259 | try 260 | { 261 | state_interface_to_states_[name] = client_->read(config); 262 | } 263 | catch (const ModbusReadException & e) 264 | { 265 | RCLCPP_WARN_STREAM( 266 | rclcpp::get_logger("ModbusHardwareInterface"), 267 | "Failed to read interface<" << name.c_str() << "> with error:" << e.what() 268 | << ". Deactivating."); 269 | return hardware_interface::return_type::DEACTIVATE; 270 | } 271 | } 272 | 273 | disconnect_non_persistent_connection(); 274 | return hardware_interface::return_type::OK; 275 | } 276 | 277 | hardware_interface::return_type ModbusHardwareInterface::write( 278 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 279 | { 280 | if (!connection_established()) 281 | { 282 | return hardware_interface::return_type::ERROR; 283 | } 284 | 285 | for (const auto & [name, config] : command_interface_to_config_) 286 | { 287 | if (config.write_this_interface()) 288 | { 289 | try 290 | { 291 | client_->write(config, static_cast(command_interface_to_commands_.at(name))); 292 | } 293 | catch (const ModbusWriteException & e) 294 | { 295 | RCLCPP_WARN_STREAM( 296 | rclcpp::get_logger("ModbusHardwareInterface"), 297 | "Failed to write interface<" << name.c_str() << "> with error:" << e.what() 298 | << ". Deactivating."); 299 | return hardware_interface::return_type::DEACTIVATE; 300 | } 301 | } 302 | } 303 | 304 | disconnect_non_persistent_connection(); 305 | return hardware_interface::return_type::OK; 306 | } 307 | 308 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_cleanup( 309 | const rclcpp_lifecycle::State & /*previous_state*/) 310 | { 311 | client_->disconnect(); 312 | if (client_->connected()) 313 | { 314 | RCLCPP_ERROR_STREAM( 315 | rclcpp::get_logger("ModbusHardwareInterface"), "could not close connection to modbus."); 316 | return CallbackReturn::ERROR; 317 | } 318 | return CallbackReturn::SUCCESS; 319 | } 320 | 321 | hardware_interface::CallbackReturn ModbusHardwareInterface::on_error( 322 | const rclcpp_lifecycle::State & /*previous_state*/) 323 | { 324 | CallbackReturn ret = CallbackReturn::SUCCESS; 325 | 326 | client_->disconnect(); 327 | if (client_->connected()) 328 | { 329 | RCLCPP_ERROR_STREAM( 330 | rclcpp::get_logger("ModbusHardwareInterface"), 331 | "ON_ERROR: could not close connection to modbus."); 332 | ret = CallbackReturn::ERROR; 333 | } 334 | return ret; 335 | } 336 | 337 | template 338 | T ModbusHardwareInterface::create_config( 339 | hardware_interface::InterfaceInfo & interface, const std::string & access_function) 340 | { 341 | // get register 342 | int reg; 343 | try 344 | { 345 | reg = std::stoi(interface.parameters["register"]); 346 | } 347 | catch (const std::invalid_argument & e) 348 | { 349 | const auto reg_str = interface.parameters["register"]; 350 | RCLCPP_ERROR_STREAM( 351 | rclcpp::get_logger("ModbusHardwareInterface"), 352 | "Could not convert register number [" << reg_str << "] to int."); 353 | throw e; 354 | } 355 | 356 | int bits_to_read = NUMBER_OF_BITS_TO_READ_DEFAULT; 357 | std::string bits_to_read_str = interface.parameters["bits_to_read"]; 358 | if (bits_to_read_str.empty()) 359 | { 360 | RCLCPP_WARN_STREAM( 361 | rclcpp::get_logger("ModbusHardwareInterface"), 362 | "bits_to_read is not set for interface[" + interface.name + "]. Use '" 363 | << NUMBER_OF_BITS_TO_READ_DEFAULT << "' as default."); 364 | } 365 | else 366 | { 367 | try 368 | { 369 | bits_to_read = std::stoi(bits_to_read_str); 370 | } 371 | catch (const std::invalid_argument & e) 372 | { 373 | RCLCPP_ERROR_STREAM( 374 | rclcpp::get_logger("ModbusHardwareInterface"), 375 | "Could not convert given bits to read string:[" << bits_to_read_str << "] to an int."); 376 | throw e; 377 | } 378 | } 379 | 380 | // conversion_fn, set to default if not provided 381 | std::string conversion_fn = interface.parameters["conversion_fn"]; 382 | if (conversion_fn.empty()) 383 | { 384 | RCLCPP_WARN_STREAM( 385 | rclcpp::get_logger("ModbusHardwareInterface"), 386 | "conversion_fn is empty for interface[" + interface.name + "]. Use '" << CONVERSION_FN_DEFAULT 387 | << "' as default."); 388 | conversion_fn = CONVERSION_FN_DEFAULT; 389 | } 390 | 391 | std::string offset_str = interface.parameters["offset"]; 392 | double offset = 0.0; 393 | if (!offset_str.empty()) 394 | { 395 | offset = std::stod(offset_str); 396 | } 397 | 398 | std::string factor_str = interface.parameters["factor"]; 399 | double factor = 1.0; 400 | if (!factor_str.empty()) 401 | { 402 | factor = std::stod(factor_str); 403 | } 404 | 405 | return T(reg, bits_to_read, access_function, conversion_fn, offset, factor); 406 | } 407 | 408 | bool ModbusHardwareInterface::connection_established() const 409 | { 410 | if (client_->is_persistent_connection()) 411 | { 412 | return client_->connected(); 413 | } 414 | return client_->connect(); 415 | } 416 | 417 | void ModbusHardwareInterface::disconnect_non_persistent_connection() const 418 | { 419 | if (!client_->is_persistent_connection()) 420 | { 421 | client_->disconnect(); 422 | } 423 | } 424 | 425 | } // namespace modbus_hardware_interface 426 | 427 | #include "pluginlib/class_list_macros.hpp" 428 | 429 | PLUGINLIB_EXPORT_CLASS( 430 | modbus_hardware_interface::ModbusHardwareInterface, hardware_interface::SystemInterface) 431 | -------------------------------------------------------------------------------- /src/modbus_mock_server_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "hardware_interface/component_parser.hpp" 24 | #include "hardware_interface/hardware_info.hpp" 25 | #include "rclcpp/rclcpp.hpp" 26 | 27 | // needs to be here because defines of TRUE FALSE in modbus.h and mimic joints 28 | #include // NOLINT 29 | 30 | #define DEFAULT_REGISTER_SIZE 2 31 | 32 | struct RegisterWrapper 33 | { 34 | explicit RegisterWrapper(const int & reg, const int & reg_size) 35 | : register_(reg), register_size_(reg_size) 36 | { 37 | } 38 | int register_; 39 | int register_size_ = DEFAULT_REGISTER_SIZE; 40 | }; 41 | 42 | class ModbusServerNode : public rclcpp::Node 43 | { 44 | public: 45 | ModbusServerNode() : Node("modbus_server_node"), run_server_(true) 46 | { 47 | this->declare_parameter("modbus_server_retry_period_ms", 1000); 48 | this->declare_parameter("log_connection_reset", false); 49 | this->declare_parameter("robot_description", ""); 50 | 51 | modbus_server_retry_period_ms_ = 52 | static_cast(this->get_parameter("modbus_server_retry_period_ms").as_int()); 53 | log_connection_reset_ = this->get_parameter("log_connection_reset").as_bool(); 54 | std::string robot_description = this->get_parameter("robot_description").as_string(); 55 | if (robot_description.empty()) 56 | { 57 | throw std::runtime_error( 58 | std::string(this->get_name()) + 59 | ": no robot_description file provided, cannot configure server."); 60 | } 61 | 62 | // Attempt initial setup 63 | // for now we are just interested in the first since there is only one. 64 | // TODO(Manual) make generic 65 | hardware_interface::HardwareInfo info = 66 | hardware_interface::parse_control_resources_from_urdf(robot_description)[0]; 67 | config_server_based_on_urdf(info); 68 | retryModbusSetup(ip_address_, port_); 69 | 70 | // Start the server in a new thread 71 | std::thread server_thread(&ModbusServerNode::runServer, this); 72 | server_thread.detach(); 73 | } 74 | 75 | void config_server_based_on_urdf(hardware_interface::HardwareInfo & info) 76 | { 77 | ip_address_ = info.hardware_parameters["modbus_server_ip"]; 78 | 79 | if (ip_address_.empty()) 80 | { 81 | throw std::runtime_error( 82 | "ModbusMockServer: modbus_server_ip is empty for Hardware [" + info.name + 83 | "]. Need to specify the ip of the server "); 84 | } 85 | 86 | // Get port of server for each joint 87 | // catch and rethrow to provide some additional information for user where it failed since we 88 | // don't provide default value for the port and crash in case the port is unset/invalid 89 | try 90 | { 91 | port_ = std::stoi(info.hardware_parameters["modbus_server_port"]); 92 | } 93 | catch (const std::invalid_argument & e) 94 | { 95 | throw std::invalid_argument( 96 | "ModbusMockServer: Error while getting port for Hardware[" + info.name + "] " + e.what()); 97 | } 98 | catch (const std::out_of_range & e) 99 | { 100 | throw std::out_of_range( 101 | "ModbusMockServer: Error while getting port for Hardware[" + info.name + "] " + e.what()); 102 | } 103 | 104 | uint8_t highest_register = std::numeric_limits::min(); 105 | for (hardware_interface::ComponentInfo & joint : info.joints) 106 | { 107 | for (auto & state_interface : joint.state_interfaces) 108 | { 109 | std::string reg_str = state_interface.parameters["register"]; 110 | // not used with modbus 111 | if (reg_str.empty()) 112 | { 113 | continue; 114 | } 115 | 116 | int reg = std::stoi(reg_str); 117 | 118 | if (reg > highest_register) 119 | { 120 | highest_register = static_cast(reg); 121 | } 122 | } 123 | 124 | for (auto & command_interface : joint.command_interfaces) 125 | { 126 | std::string reg_str = command_interface.parameters["register"]; 127 | // not used with modbus 128 | if (reg_str.empty()) 129 | { 130 | continue; 131 | } 132 | int reg = std::stoi(reg_str); 133 | if (reg > highest_register) 134 | { 135 | highest_register = static_cast(reg); 136 | } 137 | } 138 | } 139 | setupModbusServer(highest_register); 140 | } 141 | 142 | bool initializeModbusContext(const std::string & ip_address, int port) 143 | { 144 | try 145 | { 146 | ctx_ = modbus_new_tcp(ip_address.c_str(), port); 147 | if (ctx_ == NULL) 148 | { 149 | RCLCPP_ERROR(this->get_logger(), "Unable to create the libmodbus context"); 150 | return false; 151 | } 152 | RCLCPP_INFO(this->get_logger(), "Modbus TCP context created"); 153 | 154 | server_socket_ = modbus_tcp_listen(ctx_, 1); 155 | if (server_socket_ == -1) 156 | { 157 | RCLCPP_ERROR(this->get_logger(), "Failed to listen on port %d", port); 158 | modbus_free(ctx_); 159 | ctx_ = NULL; 160 | return false; 161 | } 162 | RCLCPP_INFO(this->get_logger(), "Listening on port %d", port); 163 | return true; 164 | } 165 | catch (const std::exception & e) 166 | { 167 | RCLCPP_ERROR(this->get_logger(), "Exception in initializeModbusContext: %s", e.what()); 168 | cleanupModbus(); 169 | return false; 170 | } 171 | } 172 | 173 | bool setupModbusServer(uint8_t highest_register) 174 | { 175 | try 176 | { 177 | if (highest_register < std::numeric_limits::max() - 1) 178 | { 179 | // Add one since if we get 12 as register we want to have 12 available and not only 11 180 | highest_register = highest_register + 2; 181 | } 182 | RCLCPP_ERROR(this->get_logger(), "Highest register:%i", highest_register); 183 | // TODO(Manual) add this as parameter or yaml i don't know 184 | mb_mapping_ = 185 | modbus_mapping_new(highest_register, highest_register, highest_register, highest_register); 186 | if (mb_mapping_ == NULL) 187 | { 188 | RCLCPP_ERROR(this->get_logger(), "Failed to create Modbus mapping"); 189 | modbus_close(ctx_); 190 | modbus_free(ctx_); 191 | ctx_ = NULL; 192 | return false; 193 | } 194 | for (uint8_t i = 0; i < highest_register; ++i) 195 | { 196 | mb_mapping_->tab_bits[i] = i; 197 | mb_mapping_->tab_input_bits[i] = i; 198 | mb_mapping_->tab_input_registers[i] = i; 199 | mb_mapping_->tab_registers[i] = i; 200 | } 201 | RCLCPP_INFO(this->get_logger(), "Modbus mapping created"); 202 | return true; 203 | } 204 | catch (const std::exception & e) 205 | { 206 | RCLCPP_ERROR(this->get_logger(), "Exception in setupModbusServer: %s", e.what()); 207 | cleanupModbus(); 208 | return false; 209 | } 210 | } 211 | 212 | bool retryModbusSetup( 213 | const std::string & ip_address, int port, int max_retries = 3, int retry_delay_ms = 1000) 214 | { 215 | int attempts = 0; 216 | while (attempts < max_retries) 217 | { 218 | if (initializeModbusContext(ip_address, port)) 219 | { 220 | return true; // Successful setup 221 | } 222 | attempts++; 223 | RCLCPP_WARN( 224 | this->get_logger(), "Retrying setup (attempt %d of %d) in %d milliseconds", attempts, 225 | max_retries, retry_delay_ms); 226 | std::this_thread::sleep_for(std::chrono::milliseconds(retry_delay_ms)); 227 | } 228 | RCLCPP_ERROR( 229 | this->get_logger(), "Failed to set up Modbus server after %d attempts", max_retries); 230 | return false; // Return false if all attempts fail 231 | } 232 | 233 | bool isServerReady() { return ctx_ != nullptr && mb_mapping_ != nullptr; } 234 | 235 | void runServer() 236 | { 237 | RCLCPP_INFO(this->get_logger(), "Modbus server is ready to accept connections"); 238 | // Main server loop for accepting clients 239 | while (run_server_ && rclcpp::ok()) 240 | { 241 | if (!isServerReady()) 242 | { 243 | RCLCPP_WARN(this->get_logger(), "Server not ready to handle requests"); 244 | retryModbusSetup(ip_address_, port_, 1, modbus_server_retry_period_ms_); 245 | continue; 246 | } 247 | 248 | int client_socket = modbus_tcp_accept(ctx_, &server_socket_); 249 | if (client_socket == -1) 250 | { 251 | if (errno == EAGAIN || errno == EWOULDBLOCK) 252 | { 253 | // No client connection attempt, continue 254 | continue; 255 | } 256 | else 257 | { 258 | // Log error and break out of the loop 259 | RCLCPP_ERROR( 260 | this->get_logger(), "Error accepting client connection: %s", modbus_strerror(errno)); 261 | cleanupModbus(); // Clean up resources 262 | retryModbusSetup(ip_address_, port_); 263 | } 264 | } 265 | 266 | // Create a new thread for each client connection 267 | std::thread client_thread = std::thread(&ModbusServerNode::handleClient, this, client_socket); 268 | client_thread.detach(); 269 | } 270 | } 271 | 272 | void handleClient(int client_socket) 273 | { 274 | // Create a new Modbus context for each client 275 | modbus_t * client_ctx = modbus_new_tcp(ip_address_.c_str(), port_); 276 | if (!client_ctx) 277 | { 278 | RCLCPP_ERROR(this->get_logger(), "Failed to create context for client"); 279 | close(client_socket); 280 | return; 281 | } 282 | RCLCPP_INFO(this->get_logger(), "Got new connection!"); 283 | // Set the new file descriptor for the client context 284 | modbus_set_socket(client_ctx, client_socket); 285 | 286 | while (run_server_ && rclcpp::ok()) 287 | { 288 | uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH]; 289 | int rc = modbus_receive(client_ctx, query); 290 | 291 | // Convert the query array elements to integers and print them 292 | std::string query_str; 293 | for (int i = 0; i < MODBUS_TCP_MAX_ADU_LENGTH; ++i) 294 | { 295 | query_str += std::to_string(query[i]) + ", "; 296 | } 297 | 298 | // Print the value of rc and the content of the query array 299 | RCLCPP_INFO( 300 | this->get_logger(), "New query return is %i with query %s", rc, query_str.c_str()); 301 | if (rc == -1) 302 | { 303 | // Log error and break out of the loop 304 | if (errno == ECONNRESET) 305 | { 306 | if (log_connection_reset_) 307 | { 308 | RCLCPP_WARN(this->get_logger(), "Connection reset by peer, client disconnected"); 309 | } 310 | } 311 | else 312 | { 313 | RCLCPP_ERROR(this->get_logger(), "Error receiving data: %s", modbus_strerror(errno)); 314 | } 315 | break; 316 | } 317 | 318 | // Lock resources while processing the request 319 | std::lock_guard lock(resource_mutex_); 320 | modbus_reply(client_ctx, query, rc, mb_mapping_); 321 | } 322 | 323 | // Clean up client context and socket 324 | modbus_close(client_ctx); 325 | modbus_free(client_ctx); 326 | close(client_socket); 327 | } 328 | 329 | void cleanupModbus() 330 | { 331 | RCLCPP_INFO(this->get_logger(), "Cleaning up Modbus resources"); 332 | if (mb_mapping_) 333 | { 334 | modbus_mapping_free(mb_mapping_); 335 | mb_mapping_ = nullptr; 336 | } 337 | if (ctx_) 338 | { 339 | modbus_close(ctx_); 340 | modbus_free(ctx_); 341 | ctx_ = nullptr; 342 | } 343 | RCLCPP_INFO(this->get_logger(), "Modbus resources cleaned up"); 344 | } 345 | 346 | ~ModbusServerNode() 347 | { 348 | run_server_ = false; // Signal all threads to stop 349 | cleanupModbus(); 350 | } 351 | 352 | private: 353 | std::atomic run_server_; // Atomic flag to control server running state 354 | int server_socket_; 355 | std::mutex resource_mutex_; 356 | 357 | // parameters 358 | std::string ip_address_; 359 | int port_; 360 | int modbus_server_retry_period_ms_; 361 | bool log_connection_reset_; // Flag to control logging of connection reset errors 362 | 363 | // modbus 364 | modbus_t * ctx_; 365 | modbus_mapping_t * mb_mapping_; 366 | uint16_t counter_; 367 | 368 | // Atomic flag to control server running state 369 | }; 370 | 371 | int main(int argc, char * argv[]) 372 | { 373 | rclcpp::init(argc, argv); 374 | std::shared_ptr executor = 375 | std::make_shared(); 376 | auto modbus_server = std::make_shared(); 377 | executor->add_node(modbus_server); 378 | executor->spin(); 379 | rclcpp::shutdown(); 380 | return 0; 381 | } 382 | -------------------------------------------------------------------------------- /test/test_modbus_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "hardware_interface/resource_manager.hpp" 20 | #include "ros2_control_test_assets/components_urdfs.hpp" 21 | #include "ros2_control_test_assets/descriptions.hpp" 22 | 23 | class TestModbusHardwareInterface : public ::testing::Test 24 | { 25 | protected: 26 | void SetUp() override 27 | { 28 | // TODO(anyone): Extend this description to your robot 29 | valid_modbus_hardware = 30 | R"( 31 | 32 | 33 | modbus_hardware_interface/ModbusHardwareInterface 34 | 127.0.0.1 35 | 1234 36 | true 37 | 38 | 39 | 40 | 1 41 | 32 42 | float_abcd 43 | register 44 | 45 | 46 | 2 47 | 1 48 | float 49 | bits 50 | 51 | 52 | 53 | 2 54 | 1 55 | float 56 | input_bits 57 | 58 | 59 | 32 60 | 32 61 | float_abcd 62 | input_register 63 | 64 | 65 | 66 | )"; 67 | } 68 | std::string valid_modbus_hardware; 69 | }; 70 | 71 | TEST_F(TestModbusHardwareInterface, load_modbus_hardware_interface_2dof) 72 | { 73 | auto urdf = ros2_control_test_assets::urdf_head + valid_modbus_hardware + 74 | ros2_control_test_assets::urdf_tail; 75 | ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); 76 | } 77 | --------------------------------------------------------------------------------