├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── figs └── omni_wheel_fig.png ├── include └── omni_wheel_controller │ ├── odometry.hpp │ ├── omni_wheel_controller.hpp │ ├── speed_limiter.hpp │ └── visibility_control.h ├── omni_wheel_plugin.xml ├── package.xml ├── src ├── odometry.cpp ├── omni_wheel_controller.cpp ├── omni_wheel_controller_parameter.yaml └── speed_limiter.cpp └── test ├── config └── test_omni_wheel_controller.yaml ├── descriptions.hpp ├── test_load_omni_wheel_controller.cpp └── test_omni_wheel_controller.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(omni_wheel_controller LANGUAGES CXX) 3 | 4 | if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") 5 | add_compile_options(-Wall -Wextra -Wconversion) 6 | endif() 7 | 8 | set(THIS_PACKAGE_INCLUDE_DEPENDS 9 | controller_interface 10 | generate_parameter_library 11 | geometry_msgs 12 | hardware_interface 13 | nav_msgs 14 | pluginlib 15 | rclcpp 16 | rclcpp_lifecycle 17 | rcpputils 18 | realtime_tools 19 | tf2 20 | tf2_msgs 21 | ) 22 | 23 | find_package(ament_cmake REQUIRED) 24 | find_package(backward_ros REQUIRED) 25 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 26 | find_package(${Dependency} REQUIRED) 27 | endforeach() 28 | 29 | generate_parameter_library(omni_wheel_controller_parameters 30 | src/omni_wheel_controller_parameter.yaml 31 | ) 32 | 33 | add_library(omni_wheel_controller SHARED 34 | src/omni_wheel_controller.cpp 35 | src/odometry.cpp 36 | src/speed_limiter.cpp 37 | ) 38 | target_compile_features(omni_wheel_controller PUBLIC cxx_std_17) 39 | target_include_directories(omni_wheel_controller PUBLIC 40 | $ 41 | $ 42 | $ 43 | ) 44 | target_link_libraries(omni_wheel_controller PUBLIC omni_wheel_controller_parameters) 45 | ament_target_dependencies(omni_wheel_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) 46 | # Causes the visibility macros to use dllexport rather than dllimport, 47 | # which is appropriate when building the dll but not consuming it. 48 | target_compile_definitions(omni_wheel_controller PRIVATE "OMNI_WHEEL_CONTROLLER_BUILDING_DLL") 49 | pluginlib_export_plugin_description_file(controller_interface omni_wheel_plugin.xml) 50 | 51 | if(BUILD_TESTING) 52 | find_package(ament_cmake_gmock REQUIRED) 53 | find_package(controller_manager REQUIRED) 54 | find_package(ros2_control_test_assets REQUIRED) 55 | 56 | ament_add_gmock(test_omni_wheel_controller 57 | test/test_omni_wheel_controller.cpp 58 | ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_omni_wheel_controller.yaml) 59 | target_link_libraries(test_omni_wheel_controller 60 | omni_wheel_controller 61 | ) 62 | ament_target_dependencies(test_omni_wheel_controller 63 | geometry_msgs 64 | hardware_interface 65 | nav_msgs 66 | rclcpp 67 | rclcpp_lifecycle 68 | realtime_tools 69 | tf2 70 | tf2_msgs 71 | ) 72 | 73 | ament_add_gmock(test_load_omni_wheel_controller 74 | test/test_load_omni_wheel_controller.cpp 75 | ) 76 | target_include_directories(test_load_omni_wheel_controller PUBLIC 77 | test) 78 | ament_target_dependencies(test_load_omni_wheel_controller 79 | controller_manager 80 | ros2_control_test_assets 81 | ) 82 | endif() 83 | 84 | install( 85 | DIRECTORY include/ 86 | DESTINATION include/omni_wheel_controller 87 | ) 88 | install(TARGETS omni_wheel_controller omni_wheel_controller_parameters 89 | EXPORT export_omni_wheel_controller 90 | RUNTIME DESTINATION bin 91 | ARCHIVE DESTINATION lib 92 | LIBRARY DESTINATION lib 93 | ) 94 | 95 | ament_export_targets(export_omni_wheel_controller HAS_LIBRARY_TARGET) 96 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 97 | ament_package() 98 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # omni_wheel_controller 2 | Omni wheel controllers to accompany ros2_control (unofficial) 3 | This package is based on diff_drive_controller. 4 | 5 | - This package checked in docker "ros:humble" image. Sample code is [here](https://github.com/hijimasa/omni_wheel_controller_sample). 6 | 7 | # Overview 8 | 9 | Controller for omni wheel robot. Control is in the form of a velocity command, that is split then sent on the wheels of a omni wheel robot. Odometry is computed from the feedback from the hardware, and published. 10 | 11 | ## Velocity commands 12 | 13 | The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. 14 | 15 | ## Hardware interface type 16 | 17 | The controller works with wheel joints through a velocity interface. 18 | 19 | ## Other features 20 | 21 | - Realtime-safe implementation. 22 | - Odometry publishing 23 | - Task-space velocity, acceleration and jerk limits 24 | - Automatic stop after command time-out 25 | 26 | ## Mathematical Background 27 | The illustration on the below shows the pattern diagram of one wheel in the Omni wheel robot. 28 | ![pattern diagram of one wheel](figs/omni_wheel_fig.png) 29 | In this diagram, the following notation applies. 30 | 31 | - $V$ 32 | 33 | Robot's linear velocity 34 | 35 | - $\Omega$ 36 | 37 | Robot's angular velocity 38 | 39 | - $x_n, y_n$ 40 | 41 | n-th omni wheel position. 42 | 43 | - $l_n$ 44 | 45 | Distance between n-th omni wheel and robot's center. 46 | $l_n = \sqrt{x_n^2+y_n^2}$ 47 | This package expects same distance $l$. 48 | 49 | - $r$ 50 | Omni wheel's radius. 51 | 52 | - $\theta_n$ 53 | n-th omni wheel's direction 54 | In this package, the omniwheel is expected to be perpendicular to the direction vector from the center of the robot. 55 | 56 | - $\omega_n$ 57 | n-th omni wheel's velocity 58 | 59 | This controller calculates the n-th omni wheel's velocity $\omega_n$ to: 60 | $\omega_n = -\frac{1}{r}(V_x \cos{\theta_n} + V_y \sin{\theta_n} + \Omega l \sin{(\theta_n - \arctan{\frac{y_n}{x_n}})})$ 61 | 62 | # ROS API 63 | 64 | ## Description 65 | 66 | The controller main input is a geometry_msgs::Twist topic in the namespace of the controller. 67 | 68 | ### Subscribed Topics 69 | 70 | - cmd_vel ([geometry_msgs/Twist](http://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html)) 71 | 72 | Velocity command. 73 | 74 | ### Published Topics 75 | 76 | - odom ([nav_msgs/Odometry](http://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html)) 77 | 78 | Odometry computed from the hardware feedback. 79 | 80 | - /tf ([tf/tfMessage](http://docs.ros.org/en/api/tf/html/msg/tfMessage.html)) 81 | 82 | Transform from odom to base_footprint 83 | 84 | - publish_cmd ([geometry_msgs/TwistStamped](http://docs.ros.org/en/api/geometry_msgs/html/msg/TwistStamped.html)) 85 | 86 | Available when "publish_cmd" parameter is set to True. It is the Twist after limiters have been applied on the controller input. 87 | 88 | ## Parameters 89 | - omni_wheel_names (string | string[...]) 90 | 91 | Omni wheel joint names list 92 | 93 | - omni_wheel_angle (double | double[...]) 94 | 95 | Omni wheel direction $\theta_n$ list 96 | 97 | - pose_covariance_diagonal (double[6]) 98 | 99 | Diagonal of the covariance matrix for odometry pose publishing 100 | 101 | - twist_covariance_diagonal (double[6]) 102 | 103 | Diagonal of the covariance matrix for odometry twist publishing 104 | 105 | - publish_rate (double, default: 50.0) 106 | 107 | Frequency (in Hz) at which the odometry is published. Used for both tf and odom 108 | 109 | - cmd_vel_timeout (double, default: 0.5) 110 | 111 | Allowed period (in s) allowed between two successive velocity commands. After this delay, a zero speed command will be sent to the wheels. 112 | 113 | - base_frame_id (string, default: base_link) 114 | 115 | Base frame_id, which is used to fill in the child_frame_id of the Odometry messages and TF. 116 | 117 | - linear/x/has_velocity_limits (bool, default: false) 118 | 119 | Whether the controller should limit linear speed or not. 120 | 121 | - linear/x/max_velocity (double) 122 | 123 | Maximum linear velocity (in m/s) 124 | 125 | - linear/x/min_velocity (double) 126 | 127 | Minimum linear velocity (in m/s). Setting this to 0.0 will disable backwards motion. When unspecified, -max_velocity is used. 128 | 129 | - linear/x/has_acceleration_limits (bool, default: false) 130 | 131 | Whether the controller should limit linear acceleration or not. 132 | 133 | - linear/x/max_acceleration (double) 134 | 135 | Maximum linear acceleration (in m/s^2) 136 | 137 | - linear/x/min_acceleration (double) 138 | 139 | Minimum linear acceleration (in m/s^2). When unspecified, -max_acceleration is used. 140 | 141 | - linear/x/has_jerk_limits (bool, default: false) 142 | 143 | Whether the controller should limit linear jerk or not. 144 | 145 | - linear/x/max_jerk (double) 146 | 147 | Maximum linear jerk (in m/s^3). 148 | 149 | - angular/z/has_velocity_limits (bool, default: false) 150 | 151 | Whether the controller should limit angular velocity or not. 152 | 153 | - angular/z/max_velocity (double) 154 | 155 | Maximum angular velocity (in rad/s) 156 | 157 | - angular/z/min_velocity (double) 158 | 159 | Minimum angular velocity (in rad/s). Setting this to 0.0 will disable counter-clockwise rotation. When unspecified, -max_velocity is used. 160 | 161 | - angular/z/has_acceleration_limits (bool, default: false) 162 | 163 | Whether the controller should limit angular acceleration or not. 164 | 165 | - angular/z/max_acceleration (double) 166 | 167 | Maximum angular acceleration (in rad/s^2) 168 | 169 | - angular/z/min_acceleration (double) 170 | 171 | Minimum angular acceleration (in rad/s^2). When unspecified, -max_acceleration is used. 172 | 173 | - angular/z/has_jerk_limits (bool, default: false) 174 | 175 | Whether the controller should limit angular jerk or not. 176 | 177 | - angular/z/max_jerk (double) 178 | 179 | Maximum angular jerk (in m/s^3). 180 | 181 | - enable_odom_tf (bool, default: true) 182 | 183 | Publish to TF directly or not 184 | 185 | - omni_wheel_distance (double) 186 | 187 | The distance between the wheel and robot's center $l$. 188 | It is expected they all have the same size. 189 | 190 | - wheel_radius (double) 191 | 192 | Radius of the wheels $r$. It is expected they all have the same size. 193 | 194 | - odom_frame_id (string, default: "/odom") 195 | 196 | Name of frame to publish odometry in. 197 | 198 | - publish_cmd (bool, default: False) 199 | 200 | Publish the velocity command to be executed. It is to monitor the effect of limiters on the controller input. 201 | 202 | - velocity_rolling_window_size (int, default: 10) 203 | 204 | The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities 205 | 206 | ## Controller configuration examples 207 | 208 | ### Minimal description 209 | T.B.D. 210 | 211 | ### Complete description 212 | 213 | ```yaml 214 | omni_wheel_controller: 215 | ros__parameters: 216 | omni_wheel_names : ['wheel0_shaft_joint', 'wheel1_shaft_joint', 'wheel2_shaft_joint', 'wheel3_shaft_joint'] 217 | omni_wheel_angle : [0.785398, 2.356194, -2.356194, -0.785398] 218 | 219 | omni_wheel_distance : 0.49 220 | wheel_radius : 0.05 221 | 222 | odom_frame_id: odom 223 | base_frame_id: base_link 224 | 225 | pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 226 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 227 | 228 | enable_odom_tf: true 229 | 230 | cmd_vel_timeout: 3.0 231 | publish_limited_velocity: true 232 | velocity_rolling_window_size: 10 233 | 234 | # limits 235 | linear.x.has_velocity_limits: true 236 | linear.x.has_acceleration_limits: false 237 | linear.x.has_jerk_limits: false 238 | linear.x.max_velocity: 1.0 239 | linear.x.min_velocity: -1.0 240 | linear.x.max_acceleration: 0.4 241 | linear.x.min_acceleration: -0.4 242 | linear.x.max_jerk: 0.5 243 | linear.x.min_jerk: -0.5 244 | 245 | angular.z.has_velocity_limits: true 246 | angular.z.has_acceleration_limits: false 247 | angular.z.has_jerk_limits: false 248 | angular.z.max_velocity: 1.5 249 | angular.z.min_velocity: -1.5 250 | angular.z.max_acceleration: 0.8 251 | angular.z.min_acceleration: -0.8 252 | angular.z.max_jerk: 0.5 253 | angular.z.min_jerk: -0.5 254 | ``` 255 | 256 | ## Note 257 | "rcppmath/rolling_mean_accumulator.hpp" is added in rcpputils ver 2.2.0 258 | 259 | "rcpputils/rolling_mean_accumulator.hpp" is added in rcpputils ver 2.6.0 260 | 261 | Note that the files that can be included depend on the version of rcpputils. 262 | -------------------------------------------------------------------------------- /figs/omni_wheel_fig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nguyen-v/omni_wheel_controller/cbbc0876d1e90bd57a133b7dc5d921dfe859d81d/figs/omni_wheel_fig.png -------------------------------------------------------------------------------- /include/omni_wheel_controller/odometry.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Luca Marchionni 17 | * Author: Bence Magyar 18 | * Author: Enrique Fernández 19 | * Author: Paul Mathieu 20 | */ 21 | 22 | #ifndef OMNI_WHEEL_CONTROLLER__ODOMETRY_HPP_ 23 | #define OMNI_WHEEL_CONTROLLER__ODOMETRY_HPP_ 24 | 25 | #include 26 | 27 | #include "rclcpp/time.hpp" 28 | //#include "rcpputils/rolling_mean_accumulator.hpp" 29 | #include "rcpputils/rolling_mean_accumulator.hpp" 30 | 31 | #include 32 | 33 | namespace omni_wheel_controller 34 | { 35 | class Odometry 36 | { 37 | public: 38 | /** 39 | * \brief Constructor 40 | * Timestamp will get the current time value 41 | * Value will be set to zero 42 | * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean 43 | */ 44 | explicit Odometry(size_t velocity_rolling_window_size = 10); 45 | 46 | /** 47 | * \brief Initialize the odometry 48 | * \param time Current time 49 | */ 50 | void init(const rclcpp::Time & time); 51 | /** 52 | * \brief Updates the odometry class with latest wheels position 53 | * \param omni_wheel_pos Omni wheel positions [rad] 54 | * \param time Current time 55 | * \return true if the odometry is actually updated 56 | */ 57 | bool update(std::vector omni_wheel_pos, const rclcpp::Time &time); 58 | /** 59 | * \brief Updates the odometry class with latest velocity command 60 | * \param linear Linear velocity [m/s] 61 | * \param angular Angular velocity [rad/s] 62 | * \param time Current time 63 | */ 64 | void updateOpenLoop(double lin_x, double lin_y, double angular, const rclcpp::Time &time); 65 | void resetOdometry(); 66 | 67 | /** 68 | * \brief x position getter 69 | * \return x position [m] 70 | */ 71 | double getX() const 72 | { 73 | return x_; 74 | } 75 | /** 76 | * \brief y position getter 77 | * \return y position [m] 78 | */ 79 | double getY() const 80 | { 81 | return y_; 82 | } 83 | /** 84 | * \brief heading getter 85 | * \return heading [rad] 86 | */ 87 | double getHeading() const { return heading_; } 88 | /** 89 | * \brief linear x velocity getter 90 | * \return linear velocity [m/s] 91 | */ 92 | double getLinearX() const 93 | { 94 | return lin_x_; 95 | } 96 | /** 97 | * \brief linear y velocity getter 98 | * \return linear velocity [m/s] 99 | */ 100 | double getLinearY() const 101 | { 102 | return lin_y_; 103 | } 104 | /** 105 | * \brief angular velocity getter 106 | * \return angular velocity [rad/s] 107 | */ 108 | double getAngular() const 109 | { 110 | return angular_; 111 | } 112 | 113 | /** 114 | * \brief motion matrix getter 115 | * \return motion matrix 116 | */ 117 | const Eigen::MatrixXd& getMotionMatrix() const 118 | { 119 | return motion_matrix_; 120 | } 121 | 122 | /** 123 | * \brief Sets the wheel parameters: radius and separation 124 | * \param wheel_separation Separation between left and right wheels [m] 125 | * \param left_wheel_radius Left wheel radius [m] 126 | * \param right_wheel_radius Right wheel radius [m] 127 | */ 128 | void setWheelParams(double omni_wheel_distance, double omni_wheel_radius, std::vector omni_wheel_yaw, std::vector omni_wheel_dir); 129 | /** 130 | * \brief Velocity rolling window size setter 131 | * \param velocity_rolling_window_size Velocity rolling window size 132 | */ 133 | void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); 134 | 135 | private: 136 | // using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; 137 | using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; 138 | 139 | void integrateRungeKutta2(double lin_x, double lin_y, double angular); 140 | void integrateExact(double lin_x, double lin_y, double angular); 141 | void resetAccumulators(); 142 | 143 | /// Current timestamp: 144 | rclcpp::Time timestamp_; 145 | 146 | /// Current pose: 147 | double x_; // [m] 148 | double y_; // [m] 149 | double heading_; // [rad] 150 | 151 | /// Current velocity: 152 | double lin_x_; // [m/s] 153 | double lin_y_; // [m/s] 154 | double angular_; // [rad/s] 155 | 156 | /// Wheel kinematic parameters [m]: 157 | Eigen::MatrixXd motion_matrix_; 158 | Eigen::MatrixXd motion_matrix_inverse_; 159 | 160 | /// Previou wheel position/state [rad]: 161 | std::vector omni_wheel_old_pos_; 162 | 163 | /// Rolling mean accumulators for the linar and angular velocities: 164 | size_t velocity_rolling_window_size_; 165 | RollingMeanAccumulator linear_x_accumulator_; 166 | RollingMeanAccumulator linear_y_accumulator_; 167 | RollingMeanAccumulator angular_accumulator_; 168 | }; 169 | 170 | } // namespace omni_wheel_controller 171 | 172 | #endif // OMNI_WHEEL_CONTROLLER__ODOMETRY_HPP_ 173 | -------------------------------------------------------------------------------- /include/omni_wheel_controller/omni_wheel_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Bence Magyar, Enrique Fernández, Manuel Meraz 17 | */ 18 | 19 | #ifndef OMNI_WHEEL_CONTROLLER__OMNI_WHEEL_CONTROLLER_HPP_ 20 | #define OMNI_WHEEL_CONTROLLER__OMNI_WHEEL_CONTROLLER_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "controller_interface/controller_interface.hpp" 30 | #include "omni_wheel_controller/odometry.hpp" 31 | #include "omni_wheel_controller/speed_limiter.hpp" 32 | #include "omni_wheel_controller/visibility_control.h" 33 | #include "geometry_msgs/msg/twist.hpp" 34 | #include "geometry_msgs/msg/twist_stamped.hpp" 35 | #include "hardware_interface/handle.hpp" 36 | #include "nav_msgs/msg/odometry.hpp" 37 | #include "odometry.hpp" 38 | #include "rclcpp/rclcpp.hpp" 39 | #include "rclcpp_lifecycle/state.hpp" 40 | #include "realtime_tools/realtime_box.h" 41 | #include "realtime_tools/realtime_buffer.h" 42 | #include "realtime_tools/realtime_publisher.h" 43 | #include "tf2_msgs/msg/tf_message.hpp" 44 | 45 | // auto-generated by generate_parameter_library 46 | #include "omni_wheel_controller_parameters.hpp" 47 | 48 | namespace omni_wheel_controller 49 | { 50 | class OmniWheelController : public controller_interface::ControllerInterface 51 | { 52 | using Twist = geometry_msgs::msg::TwistStamped; 53 | 54 | public: 55 | OMNI_WHEEL_CONTROLLER_PUBLIC 56 | OmniWheelController(); 57 | 58 | OMNI_WHEEL_CONTROLLER_PUBLIC 59 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 60 | 61 | OMNI_WHEEL_CONTROLLER_PUBLIC 62 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 63 | 64 | OMNI_WHEEL_CONTROLLER_PUBLIC 65 | controller_interface::return_type update( 66 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 67 | 68 | OMNI_WHEEL_CONTROLLER_PUBLIC 69 | controller_interface::CallbackReturn on_init() override; 70 | 71 | OMNI_WHEEL_CONTROLLER_PUBLIC 72 | controller_interface::CallbackReturn on_configure( 73 | const rclcpp_lifecycle::State & previous_state) override; 74 | 75 | OMNI_WHEEL_CONTROLLER_PUBLIC 76 | controller_interface::CallbackReturn on_activate( 77 | const rclcpp_lifecycle::State & previous_state) override; 78 | 79 | OMNI_WHEEL_CONTROLLER_PUBLIC 80 | controller_interface::CallbackReturn on_deactivate( 81 | const rclcpp_lifecycle::State & previous_state) override; 82 | 83 | OMNI_WHEEL_CONTROLLER_PUBLIC 84 | controller_interface::CallbackReturn on_cleanup( 85 | const rclcpp_lifecycle::State & previous_state) override; 86 | 87 | OMNI_WHEEL_CONTROLLER_PUBLIC 88 | controller_interface::CallbackReturn on_error( 89 | const rclcpp_lifecycle::State & previous_state) override; 90 | 91 | OMNI_WHEEL_CONTROLLER_PUBLIC 92 | controller_interface::CallbackReturn on_shutdown( 93 | const rclcpp_lifecycle::State & previous_state) override; 94 | 95 | protected: 96 | struct WheelHandle 97 | { 98 | std::reference_wrapper feedback; 99 | std::reference_wrapper velocity; 100 | }; 101 | 102 | const char * feedback_type() const; 103 | controller_interface::CallbackReturn configure_handles( 104 | const std::vector & wheel_names, 105 | std::vector & registered_handles); 106 | 107 | std::vector registered_omni_wheel_handles_; 108 | 109 | // Parameters from ROS for omni_wheel_controller 110 | std::shared_ptr param_listener_; 111 | Params params_; 112 | 113 | Odometry odometry_; 114 | 115 | // Timeout to consider cmd_vel commands old 116 | std::chrono::milliseconds cmd_vel_timeout_{500}; 117 | 118 | std::shared_ptr> odometry_publisher_ = nullptr; 119 | std::shared_ptr> 120 | realtime_odometry_publisher_ = nullptr; 121 | 122 | std::shared_ptr> odometry_transform_publisher_ = 123 | nullptr; 124 | std::shared_ptr> 125 | realtime_odometry_transform_publisher_ = nullptr; 126 | 127 | bool subscriber_is_active_ = false; 128 | rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; 129 | rclcpp::Subscription::SharedPtr 130 | velocity_command_unstamped_subscriber_ = nullptr; 131 | 132 | realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; 133 | 134 | std::queue previous_commands_; // last two commands 135 | 136 | // speed limiters 137 | SpeedLimiter limiter_linear_x_; 138 | SpeedLimiter limiter_linear_y_; 139 | SpeedLimiter limiter_angular_; 140 | 141 | bool publish_limited_velocity_ = false; 142 | std::shared_ptr> limited_velocity_publisher_ = nullptr; 143 | std::shared_ptr> realtime_limited_velocity_publisher_ = 144 | nullptr; 145 | 146 | rclcpp::Time previous_update_timestamp_{0}; 147 | 148 | // publish rate limiter 149 | double publish_rate_ = 50.0; 150 | rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); 151 | rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; 152 | 153 | bool is_halted = false; 154 | bool use_stamped_vel_ = true; 155 | 156 | bool reset(); 157 | void halt(); 158 | }; 159 | } // namespace omni_wheel_controller 160 | #endif // OMNI_WHEEL_CONTROLLER__OMNI_WHEEL_CONTROLLER_HPP_ 161 | -------------------------------------------------------------------------------- /include/omni_wheel_controller/speed_limiter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Enrique Fernández 17 | */ 18 | 19 | #ifndef OMNI_WHEEL_CONTROLLER__SPEED_LIMITER_HPP_ 20 | #define OMNI_WHEEL_CONTROLLER__SPEED_LIMITER_HPP_ 21 | 22 | #include 23 | 24 | namespace omni_wheel_controller 25 | { 26 | class SpeedLimiter 27 | { 28 | public: 29 | /** 30 | * \brief Constructor 31 | * \param [in] has_velocity_limits if true, applies velocity limits 32 | * \param [in] has_acceleration_limits if true, applies acceleration limits 33 | * \param [in] has_jerk_limits if true, applies jerk limits 34 | * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 35 | * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 36 | * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 37 | * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 38 | * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 39 | * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 40 | */ 41 | SpeedLimiter( 42 | bool has_velocity_limits = false, bool has_acceleration_limits = false, 43 | bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, 44 | double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, 45 | double max_jerk = NAN); 46 | 47 | /** 48 | * \brief Limit the velocity and acceleration 49 | * \param [in, out] v Velocity [m/s] 50 | * \param [in] v0 Previous velocity to v [m/s] 51 | * \param [in] v1 Previous velocity to v0 [m/s] 52 | * \param [in] dt Time step [s] 53 | * \return Limiting factor (1.0 if none) 54 | */ 55 | double limit(double & v, double v0, double v1, double dt); 56 | 57 | /** 58 | * \brief Limit the velocity 59 | * \param [in, out] v Velocity [m/s] 60 | * \return Limiting factor (1.0 if none) 61 | */ 62 | double limit_velocity(double & v); 63 | 64 | /** 65 | * \brief Limit the acceleration 66 | * \param [in, out] v Velocity [m/s] 67 | * \param [in] v0 Previous velocity [m/s] 68 | * \param [in] dt Time step [s] 69 | * \return Limiting factor (1.0 if none) 70 | */ 71 | double limit_acceleration(double & v, double v0, double dt); 72 | 73 | /** 74 | * \brief Limit the jerk 75 | * \param [in, out] v Velocity [m/s] 76 | * \param [in] v0 Previous velocity to v [m/s] 77 | * \param [in] v1 Previous velocity to v0 [m/s] 78 | * \param [in] dt Time step [s] 79 | * \return Limiting factor (1.0 if none) 80 | * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control 81 | */ 82 | double limit_jerk(double & v, double v0, double v1, double dt); 83 | 84 | private: 85 | // Enable/Disable velocity/acceleration/jerk limits: 86 | bool has_velocity_limits_; 87 | bool has_acceleration_limits_; 88 | bool has_jerk_limits_; 89 | 90 | // Velocity limits: 91 | double min_velocity_; 92 | double max_velocity_; 93 | 94 | // Acceleration limits: 95 | double min_acceleration_; 96 | double max_acceleration_; 97 | 98 | // Jerk limits: 99 | double min_jerk_; 100 | double max_jerk_; 101 | }; 102 | 103 | } // namespace omni_wheel_controller 104 | 105 | #endif // OMNI_WHEEL_CONTROLLER__SPEED_LIMITER_HPP_ 106 | -------------------------------------------------------------------------------- /include/omni_wheel_controller/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 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 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef OMNI_WHEEL_CONTROLLER__VISIBILITY_CONTROL_H_ 23 | #define OMNI_WHEEL_CONTROLLER__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define OMNI_WHEEL_CONTROLLER_EXPORT __attribute__((dllexport)) 31 | #define OMNI_WHEEL_CONTROLLER_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define OMNI_WHEEL_CONTROLLER_EXPORT __declspec(dllexport) 34 | #define OMNI_WHEEL_CONTROLLER_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef OMNI_WHEEL_CONTROLLER_BUILDING_DLL 37 | #define OMNI_WHEEL_CONTROLLER_PUBLIC OMNI_WHEEL_CONTROLLER_EXPORT 38 | #else 39 | #define OMNI_WHEEL_CONTROLLER_PUBLIC OMNI_WHEEL_CONTROLLER_IMPORT 40 | #endif 41 | #define OMNI_WHEEL_CONTROLLER_PUBLIC_TYPE OMNI_WHEEL_CONTROLLER_PUBLIC 42 | #define OMNI_WHEEL_CONTROLLER_LOCAL 43 | #else 44 | #define OMNI_WHEEL_CONTROLLER_EXPORT __attribute__((visibility("default"))) 45 | #define OMNI_WHEEL_CONTROLLER_IMPORT 46 | #if __GNUC__ >= 4 47 | #define OMNI_WHEEL_CONTROLLER_PUBLIC __attribute__((visibility("default"))) 48 | #define OMNI_WHEEL_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define OMNI_WHEEL_CONTROLLER_PUBLIC 51 | #define OMNI_WHEEL_CONTROLLER_LOCAL 52 | #endif 53 | #define OMNI_WHEEL_CONTROLLER_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // OMNI_WHEEL_CONTROLLER__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /omni_wheel_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The omni wheel controller transforms linear and angular velocity messages into signals for each wheel(s) for a omni wheel robot. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | omni_wheel_controller 4 | 0.0.1 5 | Controller for a omni wheel mobile base. 6 | Masaaki Hijikata 7 | 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | generate_parameter_library 12 | 13 | backward_ros 14 | controller_interface 15 | geometry_msgs 16 | hardware_interface 17 | nav_msgs 18 | pluginlib 19 | rclcpp 20 | rclcpp_lifecycle 21 | rcpputils 22 | realtime_tools 23 | tf2 24 | tf2_msgs 25 | 26 | ament_cmake_gmock 27 | controller_manager 28 | ros2_control_test_assets 29 | ament_lint_auto 30 | ament_lint_common 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/odometry.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Enrique Fernández 17 | */ 18 | 19 | #include 20 | #include 21 | #include "omni_wheel_controller/odometry.hpp" 22 | 23 | #include 24 | 25 | namespace omni_wheel_controller 26 | { 27 | Odometry::Odometry(size_t velocity_rolling_window_size) 28 | : timestamp_(0.0) 29 | , x_(0.0) 30 | , y_(0.0) 31 | , heading_(0.0) 32 | , lin_x_(0.0) 33 | , lin_y_(0.0) 34 | , angular_(0.0) 35 | , velocity_rolling_window_size_(velocity_rolling_window_size) 36 | , linear_x_accumulator_(velocity_rolling_window_size) 37 | , linear_y_accumulator_(velocity_rolling_window_size) 38 | , angular_accumulator_(velocity_rolling_window_size) 39 | { 40 | } 41 | 42 | void Odometry::init(const rclcpp::Time & time) 43 | { 44 | // Reset accumulators and timestamp: 45 | resetAccumulators(); 46 | timestamp_ = time; 47 | } 48 | 49 | bool Odometry::update(std::vector omni_wheel_pos, const rclcpp::Time & time) 50 | { 51 | // We cannot estimate the speed with very small time intervals: 52 | const double dt = time.seconds() - timestamp_.seconds(); 53 | if (dt < 0.0001) 54 | { 55 | return false; // Interval too small to integrate with 56 | } 57 | 58 | /// Get current wheel joint positions: 59 | std::vector omni_wheel_cur_pos; 60 | for (size_t i = 0; i < omni_wheel_pos.size(); ++i) 61 | { 62 | omni_wheel_cur_pos.push_back( omni_wheel_pos[i]); 63 | } 64 | 65 | /// Estimate movement amount of wheels using old and current position: 66 | Eigen::VectorXd wheel_movement_vector(omni_wheel_pos.size()); 67 | for (size_t i = 0; i < omni_wheel_pos.size(); i++) 68 | { 69 | wheel_movement_vector(i) = omni_wheel_cur_pos[i] - omni_wheel_old_pos_[i]; 70 | } 71 | 72 | /// Update old position with current: 73 | omni_wheel_old_pos_ = omni_wheel_cur_pos; 74 | /// Compute linear and angular diff: 75 | Eigen::VectorXd robot_movement_vector = motion_matrix_inverse_ * wheel_movement_vector; 76 | const double lin_x = robot_movement_vector(0); 77 | const double lin_y = robot_movement_vector(1); 78 | const double angular = robot_movement_vector(2); 79 | // std::cout << "lin_x: " << lin_x << ", lin_y: " << lin_y << ", ang: " << angular << std::endl; 80 | 81 | // Integrate odometry: 82 | integrateExact(lin_x, lin_y, angular); 83 | 84 | timestamp_ = time; 85 | 86 | // Estimate speeds using a rolling mean to filter them out: 87 | linear_x_accumulator_.accumulate(lin_x / dt); 88 | linear_y_accumulator_.accumulate(lin_y / dt); 89 | angular_accumulator_.accumulate(angular / dt); 90 | 91 | lin_x_ = linear_x_accumulator_.getRollingMean(); 92 | lin_y_ = linear_y_accumulator_.getRollingMean(); 93 | angular_ = angular_accumulator_.getRollingMean(); 94 | 95 | return true; 96 | } 97 | 98 | void Odometry::updateOpenLoop(double lin_x, double lin_y, double angular, const rclcpp::Time & time) 99 | { 100 | /// Save last linear and angular velocity: 101 | lin_x_ = lin_x; 102 | lin_y_ = lin_y; 103 | angular_ = angular; 104 | 105 | /// Integrate odometry: 106 | const double dt = time.seconds() - timestamp_.seconds(); 107 | timestamp_ = time; 108 | integrateExact(lin_x * dt, lin_y * dt, angular * dt); 109 | } 110 | 111 | void Odometry::resetOdometry() 112 | { 113 | x_ = 0.0; 114 | y_ = 0.0; 115 | heading_ = 0.0; 116 | } 117 | 118 | void Odometry::setWheelParams(double omni_wheel_distance, double omni_wheel_radius, std::vector omni_wheel_yaw, std::vector omni_wheel_dir) 119 | { 120 | motion_matrix_ = Eigen::MatrixXd::Zero(omni_wheel_yaw.size(), 3); 121 | for (size_t row = 0; row < omni_wheel_yaw.size(); row++) 122 | { 123 | double roller_slip_direction = omni_wheel_yaw[row] - M_PI / 2.0; 124 | motion_matrix_(row, 0) = omni_wheel_dir[row] * cos(roller_slip_direction) / omni_wheel_radius; 125 | motion_matrix_(row, 1) = omni_wheel_dir[row] * sin(roller_slip_direction) / omni_wheel_radius; 126 | motion_matrix_(row, 2) = - omni_wheel_distance * omni_wheel_dir[row] / omni_wheel_radius; 127 | } 128 | motion_matrix_inverse_ = motion_matrix_.completeOrthogonalDecomposition().pseudoInverse(); 129 | 130 | omni_wheel_old_pos_.resize(omni_wheel_yaw.size()); 131 | } 132 | 133 | void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) 134 | { 135 | velocity_rolling_window_size_ = velocity_rolling_window_size; 136 | 137 | resetAccumulators(); 138 | } 139 | 140 | void Odometry::integrateRungeKutta2(double lin_x, double lin_y, double angular) 141 | { 142 | const double direction = heading_ + angular * 0.5; 143 | /// Runge-Kutta 2nd order integration: 144 | x_ += lin_x * cos(direction) - lin_y * sin(direction); 145 | y_ += lin_x * sin(direction) + lin_y * cos(direction); 146 | heading_ += angular; 147 | } 148 | 149 | void Odometry::integrateExact(double lin_x, double lin_y, double angular) 150 | { 151 | if (fabs(angular) < 1e-6) 152 | { 153 | integrateRungeKutta2(lin_x, lin_y, angular); 154 | } 155 | else 156 | { 157 | /// Exact integration (should solve problems when angular is zero): 158 | const double heading_old = heading_; 159 | const double r_x = lin_x/angular; 160 | const double r_y = lin_y/angular; 161 | heading_ += angular; 162 | x_ += r_x * (sin(heading_) - sin(heading_old)); 163 | x_ += r_y * (cos(heading_) - cos(heading_old)); 164 | y_ += -r_x * (cos(heading_) - cos(heading_old)); 165 | y_ += -r_y * (-sin(heading_) + sin(heading_old)); 166 | } 167 | } 168 | 169 | void Odometry::resetAccumulators() 170 | { 171 | linear_x_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); 172 | linear_y_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); 173 | angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); 174 | } 175 | 176 | } // namespace omni_wheel_controller 177 | -------------------------------------------------------------------------------- /src/omni_wheel_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Bence Magyar, Enrique Fernández, Manuel Meraz 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "omni_wheel_controller/omni_wheel_controller.hpp" 26 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 27 | #include "lifecycle_msgs/msg/state.hpp" 28 | #include "rclcpp/logging.hpp" 29 | #include "tf2/LinearMath/Quaternion.h" 30 | 31 | namespace 32 | { 33 | constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; 34 | constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; 35 | constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; 36 | constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; 37 | constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; 38 | } // namespace 39 | 40 | namespace omni_wheel_controller 41 | { 42 | using namespace std::chrono_literals; 43 | using controller_interface::interface_configuration_type; 44 | using controller_interface::InterfaceConfiguration; 45 | using hardware_interface::HW_IF_POSITION; 46 | using hardware_interface::HW_IF_VELOCITY; 47 | using lifecycle_msgs::msg::State; 48 | 49 | OmniWheelController::OmniWheelController() : controller_interface::ControllerInterface() {} 50 | 51 | const char * OmniWheelController::feedback_type() const 52 | { 53 | return HW_IF_POSITION; 54 | } 55 | 56 | controller_interface::CallbackReturn OmniWheelController::on_init() 57 | { 58 | try 59 | { 60 | // Create the parameter listener and get the parameters 61 | param_listener_ = std::make_shared(get_node()); 62 | params_ = param_listener_->get_params(); 63 | } 64 | catch (const std::exception & e) 65 | { 66 | fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 67 | return controller_interface::CallbackReturn::ERROR; 68 | } 69 | 70 | return controller_interface::CallbackReturn::SUCCESS; 71 | } 72 | 73 | InterfaceConfiguration OmniWheelController::command_interface_configuration() const 74 | { 75 | std::vector conf_names; 76 | for (const auto & joint_name : params_.omni_wheel_names) 77 | { 78 | conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); 79 | } 80 | return {interface_configuration_type::INDIVIDUAL, conf_names}; 81 | } 82 | 83 | InterfaceConfiguration OmniWheelController::state_interface_configuration() const 84 | { 85 | std::vector conf_names; 86 | for (const auto & joint_name : params_.omni_wheel_names) 87 | { 88 | conf_names.push_back(joint_name + "/" + feedback_type()); 89 | } 90 | return {interface_configuration_type::INDIVIDUAL, conf_names}; 91 | } 92 | 93 | controller_interface::return_type OmniWheelController::update( 94 | const rclcpp::Time & time, const rclcpp::Duration & period) 95 | { 96 | 97 | auto logger = get_node()->get_logger(); 98 | if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) 99 | { 100 | if (!is_halted) 101 | { 102 | halt(); 103 | is_halted = true; 104 | } 105 | return controller_interface::return_type::OK; 106 | } 107 | 108 | std::shared_ptr last_command_msg; 109 | received_velocity_msg_ptr_.get(last_command_msg); 110 | 111 | if (last_command_msg == nullptr) 112 | { 113 | RCLCPP_WARN(logger, "Velocity message received was a nullptr."); 114 | return controller_interface::return_type::ERROR; 115 | } 116 | 117 | const auto age_of_last_command = time - last_command_msg->header.stamp; 118 | // Brake if cmd_vel has timeout, override the stored command 119 | if (age_of_last_command > cmd_vel_timeout_) 120 | { 121 | last_command_msg->twist.linear.x = 0.0; 122 | last_command_msg->twist.linear.y = 0.0; 123 | last_command_msg->twist.angular.z = 0.0; 124 | } 125 | 126 | // command may be limited further by SpeedLimit, 127 | // without affecting the stored twist command 128 | Twist command = *last_command_msg; 129 | double & linear_x_command = command.twist.linear.x; 130 | double & linear_y_command = command.twist.linear.y; 131 | double & angular_command = command.twist.angular.z; 132 | 133 | previous_update_timestamp_ = time; 134 | 135 | if (params_.open_loop) 136 | { 137 | odometry_.updateOpenLoop(linear_x_command, linear_y_command, angular_command, time); 138 | } 139 | else 140 | { 141 | std::vector wheel_feedback; 142 | for (size_t index = 0; index < static_cast(params_.omni_wheel_names.size()); ++index) 143 | { 144 | wheel_feedback.push_back(registered_omni_wheel_handles_[index].feedback.get().get_value()); 145 | if (std::isnan(wheel_feedback[index])) 146 | { 147 | RCLCPP_ERROR( 148 | logger, "omni wheel %s is invalid for index [%zu]", feedback_type(), 149 | index); 150 | return controller_interface::return_type::ERROR; 151 | } 152 | } 153 | 154 | odometry_.update(wheel_feedback, time); 155 | } 156 | 157 | tf2::Quaternion orientation; 158 | orientation.setRPY(0.0, 0.0, odometry_.getHeading()); 159 | 160 | bool should_publish = false; 161 | try 162 | { 163 | if (previous_publish_timestamp_ + publish_period_ < time) 164 | { 165 | previous_publish_timestamp_ += publish_period_; 166 | should_publish = true; 167 | } 168 | } 169 | catch (const std::runtime_error &) 170 | { 171 | // Handle exceptions when the time source changes and initialize publish timestamp 172 | previous_publish_timestamp_ = time; 173 | should_publish = true; 174 | } 175 | 176 | if (should_publish) 177 | { 178 | if (realtime_odometry_publisher_->trylock()) 179 | { 180 | auto & odometry_message = realtime_odometry_publisher_->msg_; 181 | odometry_message.header.stamp = time; 182 | odometry_message.pose.pose.position.x = odometry_.getX(); 183 | odometry_message.pose.pose.position.y = odometry_.getY(); 184 | odometry_message.pose.pose.orientation.x = orientation.x(); 185 | odometry_message.pose.pose.orientation.y = orientation.y(); 186 | odometry_message.pose.pose.orientation.z = orientation.z(); 187 | odometry_message.pose.pose.orientation.w = orientation.w(); 188 | odometry_message.twist.twist.linear.x = odometry_.getLinearX(); 189 | odometry_message.twist.twist.linear.y = odometry_.getLinearY(); 190 | odometry_message.twist.twist.angular.z = odometry_.getAngular(); 191 | realtime_odometry_publisher_->unlockAndPublish(); 192 | } 193 | 194 | if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) 195 | { 196 | auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); 197 | transform.header.stamp = time; 198 | transform.transform.translation.x = odometry_.getX(); 199 | transform.transform.translation.y = odometry_.getY(); 200 | transform.transform.rotation.x = orientation.x(); 201 | transform.transform.rotation.y = orientation.y(); 202 | transform.transform.rotation.z = orientation.z(); 203 | transform.transform.rotation.w = orientation.w(); 204 | realtime_odometry_transform_publisher_->unlockAndPublish(); 205 | } 206 | } 207 | 208 | auto & last_command = previous_commands_.back().twist; 209 | auto & second_to_last_command = previous_commands_.front().twist; 210 | limiter_linear_x_.limit( 211 | linear_x_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); 212 | limiter_linear_y_.limit( 213 | linear_y_command, last_command.linear.y, second_to_last_command.linear.y, period.seconds()); 214 | limiter_angular_.limit( 215 | angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); 216 | 217 | previous_commands_.pop(); 218 | previous_commands_.emplace(command); 219 | 220 | // Publish limited velocity 221 | if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) 222 | { 223 | auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; 224 | limited_velocity_command.header.stamp = time; 225 | limited_velocity_command.twist = command.twist; 226 | realtime_limited_velocity_publisher_->unlockAndPublish(); 227 | } 228 | 229 | Eigen::VectorXd robot_movement_vector(3); 230 | robot_movement_vector(0) = command.twist.linear.x; 231 | robot_movement_vector(1) = command.twist.linear.y; 232 | robot_movement_vector(2) = command.twist.angular.z; 233 | 234 | Eigen::VectorXd wheel_movement_vector = odometry_.getMotionMatrix() * robot_movement_vector; 235 | // Compute and set wheels velocities: 236 | for (size_t index = 0; index < params_.omni_wheel_names.size(); ++index) 237 | { 238 | registered_omni_wheel_handles_[index].velocity.get().set_value(wheel_movement_vector(index)); 239 | } 240 | 241 | return controller_interface::return_type::OK; 242 | } 243 | 244 | controller_interface::CallbackReturn OmniWheelController::on_configure( 245 | const rclcpp_lifecycle::State &) 246 | { 247 | auto logger = get_node()->get_logger(); 248 | 249 | // update parameters if they have changed 250 | if (param_listener_->is_old(params_)) 251 | { 252 | params_ = param_listener_->get_params(); 253 | RCLCPP_INFO(logger, "Parameters were updated"); 254 | } 255 | 256 | if (params_.omni_wheel_names.empty()) 257 | { 258 | RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); 259 | return controller_interface::CallbackReturn::ERROR; 260 | } 261 | 262 | odometry_.setWheelParams(params_.omni_wheel_distance, params_.wheel_radius, params_.omni_wheel_angle, params_.omni_wheel_direction); 263 | odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); 264 | 265 | cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; 266 | publish_limited_velocity_ = params_.publish_limited_velocity; 267 | use_stamped_vel_ = params_.use_stamped_vel; 268 | 269 | limiter_linear_x_ = SpeedLimiter( 270 | params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, 271 | params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, 272 | params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, 273 | params_.linear.x.max_jerk); 274 | 275 | limiter_linear_y_ = SpeedLimiter( 276 | params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, 277 | params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, 278 | params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, 279 | params_.linear.x.max_jerk); 280 | 281 | limiter_angular_ = SpeedLimiter( 282 | params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, 283 | params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, 284 | params_.angular.z.max_velocity, params_.angular.z.min_acceleration, 285 | params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); 286 | 287 | if (!reset()) 288 | { 289 | return controller_interface::CallbackReturn::ERROR; 290 | } 291 | 292 | if (publish_limited_velocity_) 293 | { 294 | limited_velocity_publisher_ = 295 | get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); 296 | realtime_limited_velocity_publisher_ = 297 | std::make_shared>(limited_velocity_publisher_); 298 | } 299 | 300 | const Twist empty_twist; 301 | received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); 302 | 303 | // Fill last two commands with default constructed commands 304 | previous_commands_.emplace(empty_twist); 305 | previous_commands_.emplace(empty_twist); 306 | 307 | // initialize command subscriber 308 | if (use_stamped_vel_) 309 | { 310 | velocity_command_subscriber_ = get_node()->create_subscription( 311 | DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), 312 | [this](const std::shared_ptr msg) -> void 313 | { 314 | if (!subscriber_is_active_) 315 | { 316 | RCLCPP_WARN( 317 | get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); 318 | return; 319 | } 320 | if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) 321 | { 322 | RCLCPP_WARN_ONCE( 323 | get_node()->get_logger(), 324 | "Received TwistStamped with zero timestamp, setting it to current " 325 | "time, this message will only be shown once"); 326 | msg->header.stamp = get_node()->get_clock()->now(); 327 | } 328 | received_velocity_msg_ptr_.set(std::move(msg)); 329 | }); 330 | } 331 | else 332 | { 333 | velocity_command_unstamped_subscriber_ = 334 | get_node()->create_subscription( 335 | DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), 336 | [this](const std::shared_ptr msg) -> void 337 | { 338 | if (!subscriber_is_active_) 339 | { 340 | RCLCPP_WARN( 341 | get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); 342 | return; 343 | } 344 | 345 | // Write fake header in the stored stamped command 346 | std::shared_ptr twist_stamped; 347 | received_velocity_msg_ptr_.get(twist_stamped); 348 | twist_stamped->twist = *msg; 349 | twist_stamped->header.stamp = get_node()->get_clock()->now(); 350 | }); 351 | } 352 | 353 | // initialize odometry publisher and messasge 354 | odometry_publisher_ = get_node()->create_publisher( 355 | DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); 356 | realtime_odometry_publisher_ = 357 | std::make_shared>( 358 | odometry_publisher_); 359 | 360 | // Append the tf prefix if there is one 361 | std::string tf_prefix = ""; 362 | if (params_.tf_frame_prefix_enable) 363 | { 364 | if (params_.tf_frame_prefix != "") 365 | { 366 | tf_prefix = params_.tf_frame_prefix; 367 | } 368 | else 369 | { 370 | tf_prefix = std::string(get_node()->get_namespace()); 371 | } 372 | 373 | if (tf_prefix == "/") 374 | { 375 | tf_prefix = ""; 376 | } 377 | else 378 | { 379 | tf_prefix = tf_prefix + "/"; 380 | } 381 | } 382 | 383 | const auto odom_frame_id = tf_prefix + params_.odom_frame_id; 384 | const auto base_frame_id = tf_prefix + params_.base_frame_id; 385 | 386 | auto & odometry_message = realtime_odometry_publisher_->msg_; 387 | odometry_message.header.frame_id = odom_frame_id; 388 | odometry_message.child_frame_id = base_frame_id; 389 | 390 | // limit the publication on the topics /odom and /tf 391 | publish_rate_ = params_.publish_rate; 392 | publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); 393 | 394 | // initialize odom values zeros 395 | odometry_message.twist = 396 | geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); 397 | 398 | constexpr size_t NUM_DIMENSIONS = 6; 399 | for (size_t index = 0; index < 6; ++index) 400 | { 401 | // 0, 7, 14, 21, 28, 35 402 | const size_t diagonal_index = NUM_DIMENSIONS * index + index; 403 | odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; 404 | odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; 405 | } 406 | 407 | // initialize transform publisher and message 408 | odometry_transform_publisher_ = get_node()->create_publisher( 409 | DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); 410 | realtime_odometry_transform_publisher_ = 411 | std::make_shared>( 412 | odometry_transform_publisher_); 413 | 414 | // keeping track of odom and base_link transforms only 415 | auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; 416 | odometry_transform_message.transforms.resize(1); 417 | odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; 418 | odometry_transform_message.transforms.front().child_frame_id = base_frame_id; 419 | 420 | previous_update_timestamp_ = get_node()->get_clock()->now(); 421 | return controller_interface::CallbackReturn::SUCCESS; 422 | } 423 | 424 | controller_interface::CallbackReturn OmniWheelController::on_activate( 425 | const rclcpp_lifecycle::State &) 426 | { 427 | const auto wheel_config_result = 428 | configure_handles(params_.omni_wheel_names, registered_omni_wheel_handles_); 429 | 430 | if ( 431 | wheel_config_result == controller_interface::CallbackReturn::ERROR 432 | ) 433 | { 434 | return controller_interface::CallbackReturn::ERROR; 435 | } 436 | 437 | if (registered_omni_wheel_handles_.empty()) 438 | { 439 | RCLCPP_ERROR( 440 | get_node()->get_logger(), 441 | "Omni wheel interfaces are non existent"); 442 | return controller_interface::CallbackReturn::ERROR; 443 | } 444 | 445 | is_halted = false; 446 | subscriber_is_active_ = true; 447 | 448 | RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); 449 | return controller_interface::CallbackReturn::SUCCESS; 450 | } 451 | 452 | controller_interface::CallbackReturn OmniWheelController::on_deactivate( 453 | const rclcpp_lifecycle::State &) 454 | { 455 | subscriber_is_active_ = false; 456 | if (!is_halted) 457 | { 458 | halt(); 459 | is_halted = true; 460 | } 461 | registered_omni_wheel_handles_.clear(); 462 | return controller_interface::CallbackReturn::SUCCESS; 463 | } 464 | 465 | controller_interface::CallbackReturn OmniWheelController::on_cleanup( 466 | const rclcpp_lifecycle::State &) 467 | { 468 | if (!reset()) 469 | { 470 | return controller_interface::CallbackReturn::ERROR; 471 | } 472 | 473 | received_velocity_msg_ptr_.set(std::make_shared()); 474 | return controller_interface::CallbackReturn::SUCCESS; 475 | } 476 | 477 | controller_interface::CallbackReturn OmniWheelController::on_error(const rclcpp_lifecycle::State &) 478 | { 479 | if (!reset()) 480 | { 481 | return controller_interface::CallbackReturn::ERROR; 482 | } 483 | return controller_interface::CallbackReturn::SUCCESS; 484 | } 485 | 486 | bool OmniWheelController::reset() 487 | { 488 | odometry_.resetOdometry(); 489 | 490 | // release the old queue 491 | std::queue empty; 492 | std::swap(previous_commands_, empty); 493 | 494 | registered_omni_wheel_handles_.clear(); 495 | 496 | subscriber_is_active_ = false; 497 | velocity_command_subscriber_.reset(); 498 | velocity_command_unstamped_subscriber_.reset(); 499 | 500 | received_velocity_msg_ptr_.set(nullptr); 501 | is_halted = false; 502 | return true; 503 | } 504 | 505 | controller_interface::CallbackReturn OmniWheelController::on_shutdown( 506 | const rclcpp_lifecycle::State &) 507 | { 508 | return controller_interface::CallbackReturn::SUCCESS; 509 | } 510 | 511 | void OmniWheelController::halt() 512 | { 513 | for (const auto & wheel_handle : registered_omni_wheel_handles_) 514 | { 515 | wheel_handle.velocity.get().set_value(0.0); 516 | } 517 | } 518 | 519 | controller_interface::CallbackReturn OmniWheelController::configure_handles( 520 | const std::vector & wheel_names, 521 | std::vector & registered_handles) 522 | { 523 | auto logger = get_node()->get_logger(); 524 | 525 | if (wheel_names.empty()) 526 | { 527 | RCLCPP_ERROR(logger, "No omni wheel names specified"); 528 | return controller_interface::CallbackReturn::ERROR; 529 | } 530 | 531 | // register handles 532 | registered_handles.reserve(wheel_names.size()); 533 | for (const auto & wheel_name : wheel_names) 534 | { 535 | const auto interface_name = feedback_type(); 536 | const auto state_handle = std::find_if( 537 | state_interfaces_.cbegin(), state_interfaces_.cend(), 538 | [&wheel_name, &interface_name](const auto & interface) 539 | { 540 | return interface.get_prefix_name() == wheel_name && 541 | interface.get_interface_name() == interface_name; 542 | }); 543 | 544 | if (state_handle == state_interfaces_.cend()) 545 | { 546 | RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); 547 | return controller_interface::CallbackReturn::ERROR; 548 | } 549 | 550 | const auto command_handle = std::find_if( 551 | command_interfaces_.begin(), command_interfaces_.end(), 552 | [&wheel_name](const auto & interface) 553 | { 554 | return interface.get_prefix_name() == wheel_name && 555 | interface.get_interface_name() == HW_IF_VELOCITY; 556 | }); 557 | 558 | if (command_handle == command_interfaces_.end()) 559 | { 560 | RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); 561 | return controller_interface::CallbackReturn::ERROR; 562 | } 563 | 564 | registered_handles.emplace_back( 565 | WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); 566 | } 567 | 568 | return controller_interface::CallbackReturn::SUCCESS; 569 | } 570 | } // namespace omni_wheel_controller 571 | 572 | #include "class_loader/register_macro.hpp" 573 | 574 | CLASS_LOADER_REGISTER_CLASS( 575 | omni_wheel_controller::OmniWheelController, controller_interface::ControllerInterface) 576 | -------------------------------------------------------------------------------- /src/omni_wheel_controller_parameter.yaml: -------------------------------------------------------------------------------- 1 | omni_wheel_controller: 2 | omni_wheel_names: { 3 | type: string_array, 4 | default_value: [], 5 | description: "Link names of the omni wheels", 6 | } 7 | omni_wheel_angle: { 8 | type: double_array, 9 | default_value: [], 10 | description: "Directions of omni wheels [rad]", 11 | } 12 | omni_wheel_direction: { 13 | type: double_array, 14 | default_value: [1.0, 1.0, 1.0], 15 | description: "Spin direction. Positive means CW for positive commands when facing the wheel." 16 | } 17 | omni_wheel_distance: { 18 | type: double, 19 | default_value: 0.0, 20 | description: "Distance between omni wheel and center of robot", 21 | } 22 | wheel_radius: { 23 | type: double, 24 | default_value: 0.0, 25 | description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", 26 | } 27 | tf_frame_prefix_enable: { 28 | type: bool, 29 | default_value: true, 30 | description: "Enables or disables appending tf_prefix to tf frame id's.", 31 | } 32 | tf_frame_prefix: { 33 | type: string, 34 | default_value: "", 35 | description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", 36 | } 37 | odom_frame_id: { 38 | type: string, 39 | default_value: "odom", 40 | description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", 41 | } 42 | base_frame_id: { 43 | type: string, 44 | default_value: "base_link", 45 | description: "Name of the robot's base frame that is child of the odometry frame.", 46 | } 47 | pose_covariance_diagonal: { 48 | type: double_array, 49 | default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 50 | description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", 51 | } 52 | twist_covariance_diagonal: { 53 | type: double_array, 54 | default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 55 | description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", 56 | } 57 | open_loop: { 58 | type: bool, 59 | default_value: false, 60 | description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", 61 | } 62 | position_feedback: { 63 | type: bool, 64 | default_value: true, 65 | description: "Is there position feedback from hardware.", 66 | } 67 | enable_odom_tf: { 68 | type: bool, 69 | default_value: true, 70 | description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", 71 | } 72 | cmd_vel_timeout: { 73 | type: double, 74 | default_value: 0.5, # seconds 75 | description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", 76 | } 77 | publish_limited_velocity: { 78 | type: bool, 79 | default_value: false, 80 | description: "Publish limited velocity value.", 81 | } 82 | velocity_rolling_window_size: { 83 | type: int, 84 | default_value: 10, 85 | description: "Size of the rolling window for calculation of mean velocity use in odometry.", 86 | } 87 | use_stamped_vel: { 88 | type: bool, 89 | default_value: true, 90 | description: "Use stamp from input velocity message to calculate how old the command actually is.", 91 | } 92 | publish_rate: { 93 | type: double, 94 | default_value: 50.0, # Hz 95 | description: "Publishing rate (Hz) of the odometry and TF messages.", 96 | } 97 | linear: 98 | x: 99 | has_velocity_limits: { 100 | type: bool, 101 | default_value: false, 102 | } 103 | has_acceleration_limits: { 104 | type: bool, 105 | default_value: false, 106 | } 107 | has_jerk_limits: { 108 | type: bool, 109 | default_value: false, 110 | } 111 | max_velocity: { 112 | type: double, 113 | default_value: .NAN, 114 | } 115 | min_velocity: { 116 | type: double, 117 | default_value: .NAN, 118 | } 119 | max_acceleration: { 120 | type: double, 121 | default_value: .NAN, 122 | } 123 | min_acceleration: { 124 | type: double, 125 | default_value: .NAN, 126 | } 127 | max_jerk: { 128 | type: double, 129 | default_value: .NAN, 130 | } 131 | min_jerk: { 132 | type: double, 133 | default_value: .NAN, 134 | } 135 | angular: 136 | z: 137 | has_velocity_limits: { 138 | type: bool, 139 | default_value: false, 140 | } 141 | has_acceleration_limits: { 142 | type: bool, 143 | default_value: false, 144 | } 145 | has_jerk_limits: { 146 | type: bool, 147 | default_value: false, 148 | } 149 | max_velocity: { 150 | type: double, 151 | default_value: .NAN, 152 | } 153 | min_velocity: { 154 | type: double, 155 | default_value: .NAN, 156 | } 157 | max_acceleration: { 158 | type: double, 159 | default_value: .NAN, 160 | } 161 | min_acceleration: { 162 | type: double, 163 | default_value: .NAN, 164 | } 165 | max_jerk: { 166 | type: double, 167 | default_value: .NAN, 168 | } 169 | min_jerk: { 170 | type: double, 171 | default_value: .NAN, 172 | } 173 | -------------------------------------------------------------------------------- /src/speed_limiter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics S.L. 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 | /* 16 | * Author: Enrique Fernández 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include "omni_wheel_controller/speed_limiter.hpp" 23 | 24 | namespace omni_wheel_controller 25 | { 26 | SpeedLimiter::SpeedLimiter( 27 | bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, 28 | double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, 29 | double max_jerk) 30 | : has_velocity_limits_(has_velocity_limits), 31 | has_acceleration_limits_(has_acceleration_limits), 32 | has_jerk_limits_(has_jerk_limits), 33 | min_velocity_(min_velocity), 34 | max_velocity_(max_velocity), 35 | min_acceleration_(min_acceleration), 36 | max_acceleration_(max_acceleration), 37 | min_jerk_(min_jerk), 38 | max_jerk_(max_jerk) 39 | { 40 | // Check if limits are valid, max must be specified, min defaults to -max if unspecified 41 | if (has_velocity_limits_) 42 | { 43 | if (std::isnan(max_velocity_)) 44 | { 45 | throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); 46 | } 47 | if (std::isnan(min_velocity_)) 48 | { 49 | min_velocity_ = -max_velocity_; 50 | } 51 | } 52 | if (has_acceleration_limits_) 53 | { 54 | if (std::isnan(max_acceleration_)) 55 | { 56 | throw std::runtime_error( 57 | "Cannot apply acceleration limits if max_acceleration is not specified"); 58 | } 59 | if (std::isnan(min_acceleration_)) 60 | { 61 | min_acceleration_ = -max_acceleration_; 62 | } 63 | } 64 | if (has_jerk_limits_) 65 | { 66 | if (std::isnan(max_jerk_)) 67 | { 68 | throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); 69 | } 70 | if (std::isnan(min_jerk_)) 71 | { 72 | min_jerk_ = -max_jerk_; 73 | } 74 | } 75 | } 76 | 77 | double SpeedLimiter::limit(double & v, double v0, double v1, double dt) 78 | { 79 | const double tmp = v; 80 | 81 | limit_jerk(v, v0, v1, dt); 82 | limit_acceleration(v, v0, dt); 83 | limit_velocity(v); 84 | 85 | return tmp != 0.0 ? v / tmp : 1.0; 86 | } 87 | 88 | double SpeedLimiter::limit_velocity(double & v) 89 | { 90 | const double tmp = v; 91 | 92 | if (has_velocity_limits_) 93 | { 94 | v = std::clamp(v, min_velocity_, max_velocity_); 95 | } 96 | 97 | return tmp != 0.0 ? v / tmp : 1.0; 98 | } 99 | 100 | double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) 101 | { 102 | const double tmp = v; 103 | 104 | if (has_acceleration_limits_) 105 | { 106 | const double dv_min = min_acceleration_ * dt; 107 | const double dv_max = max_acceleration_ * dt; 108 | 109 | const double dv = std::clamp(v - v0, dv_min, dv_max); 110 | 111 | v = v0 + dv; 112 | } 113 | 114 | return tmp != 0.0 ? v / tmp : 1.0; 115 | } 116 | 117 | double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) 118 | { 119 | const double tmp = v; 120 | 121 | if (has_jerk_limits_) 122 | { 123 | const double dv = v - v0; 124 | const double dv0 = v0 - v1; 125 | 126 | const double dt2 = 2. * dt * dt; 127 | 128 | const double da_min = min_jerk_ * dt2; 129 | const double da_max = max_jerk_ * dt2; 130 | 131 | const double da = std::clamp(dv - dv0, da_min, da_max); 132 | 133 | v = v0 + dv0 + da; 134 | } 135 | 136 | return tmp != 0.0 ? v / tmp : 1.0; 137 | } 138 | 139 | } // namespace omni_wheel_controller 140 | -------------------------------------------------------------------------------- /test/config/test_omni_wheel_controller.yaml: -------------------------------------------------------------------------------- 1 | omni_wheel_controller: 2 | ros__parameters: 3 | omni_wheel_names : ['wheel0_shaft_joint', 'wheel1_shaft_joint', 'wheel2_shaft_joint', 'wheel3_shaft_joint'] 4 | omni_wheel_angle : [0.785398, 2.356194, -2.356194, -0.785398] 5 | 6 | omni_wheel_distance : 0.49 7 | wheel_radius : 0.05 8 | 9 | odom_frame_id: odom 10 | base_frame_id: base_link 11 | 12 | pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 13 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 14 | 15 | enable_odom_tf: true 16 | 17 | cmd_vel_timeout: 3.0 18 | publish_limited_velocity: true 19 | velocity_rolling_window_size: 10 20 | 21 | # limits 22 | linear.x.has_velocity_limits: true 23 | linear.x.has_acceleration_limits: false 24 | linear.x.has_jerk_limits: false 25 | linear.x.max_velocity: 1.0 26 | linear.x.min_velocity: -1.0 27 | linear.x.max_acceleration: 0.4 28 | linear.x.min_acceleration: -0.4 29 | linear.x.max_jerk: 0.5 30 | linear.x.min_jerk: -0.5 31 | 32 | angular.z.has_velocity_limits: true 33 | angular.z.has_acceleration_limits: false 34 | angular.z.has_jerk_limits: false 35 | angular.z.max_velocity: 1.5 36 | angular.z.min_velocity: -1.5 37 | angular.z.max_acceleration: 0.8 38 | angular.z.min_acceleration: -0.8 39 | angular.z.max_jerk: 0.5 40 | angular.z.min_jerk: -0.5 41 | -------------------------------------------------------------------------------- /test/descriptions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __DESCRIPTION_HPP__ 2 | #define __DESCRIPTION_HPP__ 3 | 4 | #include 5 | #include 6 | 7 | namespace ros2_control_test_assets 8 | { 9 | 10 | const auto omni_wheel_robot_urdf = 11 | R"( 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | transmission_interface/SimpleTransmission 71 | 72 | hardware_interface/VelocityJointInterface 73 | 74 | 75 | hardware_interface/VelocityJointInterface 76 | 1 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | Gazebo/Gray 101 | 0.8 102 | 0.05 103 | 0 1.0 0 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | transmission_interface/SimpleTransmission 116 | 117 | hardware_interface/VelocityJointInterface 118 | 119 | 120 | hardware_interface/VelocityJointInterface 121 | 1 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | Gazebo/Gray 146 | 0.8 147 | 0.05 148 | 0 1.0 0 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | transmission_interface/SimpleTransmission 161 | 162 | hardware_interface/VelocityJointInterface 163 | 164 | 165 | hardware_interface/VelocityJointInterface 166 | 1 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | Gazebo/Gray 191 | 0.8 192 | 0.05 193 | 0 1.0 0 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | transmission_interface/SimpleTransmission 206 | 207 | hardware_interface/VelocityJointInterface 208 | 209 | 210 | hardware_interface/VelocityJointInterface 211 | 1 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | Gazebo/Gray 236 | 0.8 237 | 0.05 238 | 0 1.0 0 239 | 240 | 241 | 242 | 243 | false 244 | 245 | 246 | 247 | 248 | 249 | test_actuator 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | test_actuator 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | test_actuator 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | test_actuator 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | )"; 289 | 290 | } 291 | 292 | #endif 293 | -------------------------------------------------------------------------------- /test/test_load_omni_wheel_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics SL. 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 | 18 | #include "controller_manager/controller_manager.hpp" 19 | #include "rclcpp/utilities.hpp" 20 | #include "rclcpp/executors.hpp" 21 | //#include "ros2_control_test_assets/descriptions.hpp" 22 | #include "descriptions.hpp" 23 | 24 | TEST(TestLoadOmniWheelController, load_controller) 25 | { 26 | rclcpp::init(0, nullptr); 27 | auto dummy_node = std::make_shared("dummy_node"); 28 | 29 | std::shared_ptr executor = 30 | std::make_shared(); 31 | 32 | controller_manager::ControllerManager cm( 33 | std::make_unique( 34 | ros2_control_test_assets::omni_wheel_robot_urdf, 35 | dummy_node->get_node_clock_interface(), // Clock interface 36 | dummy_node->get_node_logging_interface()), // Logging interface), 37 | executor, "test_controller_manager"); 38 | 39 | ASSERT_NE( 40 | cm.load_controller("test_omni_wheel_controller", "omni_wheel_controller/OmniWheelController"), 41 | nullptr); 42 | 43 | rclcpp::shutdown(); 44 | } 45 | -------------------------------------------------------------------------------- /test/test_omni_wheel_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics SL. 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 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "omni_wheel_controller/omni_wheel_controller.hpp" 25 | #include "hardware_interface/loaned_command_interface.hpp" 26 | #include "hardware_interface/loaned_state_interface.hpp" 27 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 28 | #include "lifecycle_msgs/msg/state.hpp" 29 | #include "rclcpp/rclcpp.hpp" 30 | 31 | using CallbackReturn = controller_interface::CallbackReturn; 32 | using hardware_interface::HW_IF_POSITION; 33 | using hardware_interface::HW_IF_VELOCITY; 34 | using hardware_interface::LoanedCommandInterface; 35 | using hardware_interface::LoanedStateInterface; 36 | using lifecycle_msgs::msg::State; 37 | using testing::SizeIs; 38 | 39 | class TestableOmniWheelController : public omni_wheel_controller::OmniWheelController 40 | { 41 | public: 42 | using OmniWheelController::OmniWheelController; 43 | std::shared_ptr getLastReceivedTwist() 44 | { 45 | std::shared_ptr ret; 46 | received_velocity_msg_ptr_.get(ret); 47 | return ret; 48 | } 49 | 50 | /** 51 | * @brief wait_for_twist block until a new twist is received. 52 | * Requires that the executor is not spinned elsewhere between the 53 | * message publication and the call to this function 54 | * 55 | * @return true if new twist msg was received, false if timeout 56 | */ 57 | bool wait_for_twist( 58 | rclcpp::Executor & executor, 59 | const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) 60 | { 61 | rclcpp::WaitSet wait_set; 62 | wait_set.add_subscription(velocity_command_subscriber_); 63 | 64 | if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) 65 | { 66 | executor.spin_some(); 67 | return true; 68 | } 69 | return false; 70 | } 71 | 72 | /** 73 | * @brief Used to get the real_time_odometry_publisher to verify its contents 74 | * 75 | * @return Copy of realtime_odometry_publisher_ object 76 | */ 77 | std::shared_ptr> 78 | get_rt_odom_publisher() 79 | { 80 | return realtime_odometry_publisher_; 81 | } 82 | }; 83 | 84 | class TestOmniWheelController : public ::testing::Test 85 | { 86 | protected: 87 | static void SetUpTestCase() { rclcpp::init(0, nullptr); } 88 | 89 | void SetUp() override 90 | { 91 | controller_ = std::make_unique(); 92 | 93 | pub_node = std::make_shared("velocity_publisher"); 94 | velocity_publisher = pub_node->create_publisher( 95 | controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); 96 | } 97 | 98 | static void TearDownTestCase() { rclcpp::shutdown(); } 99 | 100 | /// Publish velocity msgs 101 | /** 102 | * linear - magnitude of the linear command in the geometry_msgs::twist message 103 | * angular - the magnitude of the angular command in geometry_msgs::twist message 104 | */ 105 | void publish(double linear, double angular) 106 | { 107 | int wait_count = 0; 108 | auto topic = velocity_publisher->get_topic_name(); 109 | while (pub_node->count_subscribers(topic) == 0) 110 | { 111 | if (wait_count >= 5) 112 | { 113 | auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; 114 | throw std::runtime_error(error_msg); 115 | } 116 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 117 | ++wait_count; 118 | } 119 | 120 | geometry_msgs::msg::TwistStamped velocity_message; 121 | velocity_message.header.stamp = pub_node->get_clock()->now(); 122 | velocity_message.twist.linear.x = linear; 123 | velocity_message.twist.angular.z = angular; 124 | velocity_publisher->publish(velocity_message); 125 | } 126 | 127 | /// \brief wait for the subscriber and publisher to completely setup 128 | void waitForSetup() 129 | { 130 | constexpr std::chrono::seconds TIMEOUT{2}; 131 | auto clock = pub_node->get_clock(); 132 | auto start = clock->now(); 133 | while (velocity_publisher->get_subscription_count() <= 0) 134 | { 135 | if ((clock->now() - start) > TIMEOUT) 136 | { 137 | FAIL(); 138 | } 139 | rclcpp::spin_some(pub_node); 140 | } 141 | } 142 | 143 | void assignResourcesPosFeedback() 144 | { 145 | std::vector state_ifs; 146 | state_ifs.emplace_back(wheel0_wheel_pos_state_); 147 | state_ifs.emplace_back(wheel1_wheel_pos_state_); 148 | state_ifs.emplace_back(wheel2_wheel_pos_state_); 149 | state_ifs.emplace_back(wheel3_wheel_pos_state_); 150 | 151 | std::vector command_ifs; 152 | command_ifs.emplace_back(wheel0_wheel_vel_cmd_); 153 | command_ifs.emplace_back(wheel1_wheel_vel_cmd_); 154 | command_ifs.emplace_back(wheel2_wheel_vel_cmd_); 155 | command_ifs.emplace_back(wheel3_wheel_vel_cmd_); 156 | 157 | controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); 158 | } 159 | 160 | const std::string controller_name = "test_omni_wheel_controller"; 161 | std::unique_ptr controller_; 162 | 163 | const std::vector omni_wheel_names = {"wheel0_shaft_joint", "wheel1_shaft_joint", "wheel2_shaft_joint", "wheel3_shaft_joint"}; 164 | const std::vector omni_wheel_angle = {0.785398, 2.356194, -2.356194, -0.785398}; 165 | std::vector position_values_ = {0.1, 0.2, 0.3, 0.4}; 166 | std::vector velocity_values_ = {0.01, 0.02, 0.03, 0.04}; 167 | 168 | hardware_interface::StateInterface wheel0_wheel_pos_state_{ 169 | omni_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; 170 | hardware_interface::StateInterface wheel1_wheel_pos_state_{ 171 | omni_wheel_names[1], HW_IF_POSITION, &position_values_[1]}; 172 | hardware_interface::StateInterface wheel2_wheel_pos_state_{ 173 | omni_wheel_names[2], HW_IF_POSITION, &position_values_[2]}; 174 | hardware_interface::StateInterface wheel3_wheel_pos_state_{ 175 | omni_wheel_names[3], HW_IF_POSITION, &position_values_[3]}; 176 | 177 | hardware_interface::CommandInterface wheel0_wheel_vel_cmd_{ 178 | omni_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; 179 | hardware_interface::CommandInterface wheel1_wheel_vel_cmd_{ 180 | omni_wheel_names[1], HW_IF_VELOCITY, &velocity_values_[1]}; 181 | hardware_interface::CommandInterface wheel2_wheel_vel_cmd_{ 182 | omni_wheel_names[2], HW_IF_VELOCITY, &velocity_values_[2]}; 183 | hardware_interface::CommandInterface wheel3_wheel_vel_cmd_{ 184 | omni_wheel_names[3], HW_IF_VELOCITY, &velocity_values_[3]}; 185 | 186 | rclcpp::Node::SharedPtr pub_node; 187 | rclcpp::Publisher::SharedPtr velocity_publisher; 188 | }; 189 | 190 | TEST_F(TestOmniWheelController, configure_fails_without_parameters) 191 | { 192 | const auto ret = controller_->init(controller_name, "", 10, "", 193 | rclcpp::NodeOptions() 194 | .allow_undeclared_parameters(true) 195 | .automatically_declare_parameters_from_overrides(true)); 196 | ASSERT_EQ(ret, controller_interface::return_type::OK); 197 | 198 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); 199 | } 200 | 201 | TEST_F(TestOmniWheelController, configure_succeeds_when_wheels_are_specified) 202 | { 203 | const auto ret = controller_->init(controller_name, "", 10, "", 204 | rclcpp::NodeOptions() 205 | .allow_undeclared_parameters(true) 206 | .automatically_declare_parameters_from_overrides(true)); 207 | ASSERT_EQ(ret, controller_interface::return_type::OK); 208 | 209 | controller_->get_node()->set_parameter( 210 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 211 | 212 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 213 | 214 | ASSERT_THAT( 215 | controller_->state_interface_configuration().names, 216 | SizeIs(omni_wheel_names.size())); 217 | ASSERT_THAT( 218 | controller_->command_interface_configuration().names, 219 | SizeIs(omni_wheel_names.size())); 220 | } 221 | 222 | TEST_F(TestOmniWheelController, configure_succeeds_tf_test_prefix_false_no_namespace) 223 | { 224 | const auto ret = controller_->init(controller_name, "", 10, "", 225 | rclcpp::NodeOptions() 226 | .allow_undeclared_parameters(true) 227 | .automatically_declare_parameters_from_overrides(true)); 228 | ASSERT_EQ(ret, controller_interface::return_type::OK); 229 | 230 | std::string odom_id = "odom"; 231 | std::string base_link_id = "base_link"; 232 | std::string frame_prefix = "test_prefix"; 233 | 234 | controller_->get_node()->set_parameter( 235 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 236 | 237 | controller_->get_node()->set_parameter( 238 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); 239 | controller_->get_node()->set_parameter( 240 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 241 | controller_->get_node()->set_parameter( 242 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 243 | controller_->get_node()->set_parameter( 244 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 245 | 246 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 247 | 248 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 249 | std::string test_odom_frame_id = odometry_message.header.frame_id; 250 | std::string test_base_frame_id = odometry_message.child_frame_id; 251 | /* tf_frame_prefix_enable is false so no modifications to the frame id's */ 252 | ASSERT_EQ(test_odom_frame_id, odom_id); 253 | ASSERT_EQ(test_base_frame_id, base_link_id); 254 | } 255 | 256 | TEST_F(TestOmniWheelController, configure_succeeds_tf_test_prefix_true_no_namespace) 257 | { 258 | const auto ret = controller_->init(controller_name, "", 10, "", 259 | rclcpp::NodeOptions() 260 | .allow_undeclared_parameters(true) 261 | .automatically_declare_parameters_from_overrides(true)); 262 | ASSERT_EQ(ret, controller_interface::return_type::OK); 263 | 264 | std::string odom_id = "odom"; 265 | std::string base_link_id = "base_link"; 266 | std::string frame_prefix = "test_prefix"; 267 | 268 | controller_->get_node()->set_parameter( 269 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 270 | 271 | controller_->get_node()->set_parameter( 272 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); 273 | controller_->get_node()->set_parameter( 274 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 275 | controller_->get_node()->set_parameter( 276 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 277 | controller_->get_node()->set_parameter( 278 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 279 | 280 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 281 | 282 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 283 | std::string test_odom_frame_id = odometry_message.header.frame_id; 284 | std::string test_base_frame_id = odometry_message.child_frame_id; 285 | 286 | /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame 287 | * id's */ 288 | ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); 289 | ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); 290 | } 291 | 292 | TEST_F(TestOmniWheelController, configure_succeeds_tf_blank_prefix_true_no_namespace) 293 | { 294 | const auto ret = controller_->init(controller_name, "", 10, "", 295 | rclcpp::NodeOptions() 296 | .allow_undeclared_parameters(true) 297 | .automatically_declare_parameters_from_overrides(true)); 298 | ASSERT_EQ(ret, controller_interface::return_type::OK); 299 | 300 | std::string odom_id = "odom"; 301 | std::string base_link_id = "base_link"; 302 | std::string frame_prefix = ""; 303 | 304 | controller_->get_node()->set_parameter( 305 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 306 | 307 | controller_->get_node()->set_parameter( 308 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); 309 | controller_->get_node()->set_parameter( 310 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 311 | controller_->get_node()->set_parameter( 312 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 313 | controller_->get_node()->set_parameter( 314 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 315 | 316 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 317 | 318 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 319 | std::string test_odom_frame_id = odometry_message.header.frame_id; 320 | std::string test_base_frame_id = odometry_message.child_frame_id; 321 | /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame 322 | * id's */ 323 | ASSERT_EQ(test_odom_frame_id, odom_id); 324 | ASSERT_EQ(test_base_frame_id, base_link_id); 325 | } 326 | 327 | TEST_F(TestOmniWheelController, configure_succeeds_tf_test_prefix_false_set_namespace) 328 | { 329 | std::string test_namespace = "/test_namespace"; 330 | 331 | const auto ret = controller_->init(controller_name, "", 10, test_namespace, 332 | rclcpp::NodeOptions() 333 | .allow_undeclared_parameters(true) 334 | .automatically_declare_parameters_from_overrides(true)); 335 | ASSERT_EQ(ret, controller_interface::return_type::OK); 336 | 337 | std::string odom_id = "odom"; 338 | std::string base_link_id = "base_link"; 339 | std::string frame_prefix = "test_prefix"; 340 | 341 | controller_->get_node()->set_parameter( 342 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 343 | 344 | controller_->get_node()->set_parameter( 345 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); 346 | controller_->get_node()->set_parameter( 347 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 348 | controller_->get_node()->set_parameter( 349 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 350 | controller_->get_node()->set_parameter( 351 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 352 | 353 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 354 | 355 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 356 | std::string test_odom_frame_id = odometry_message.header.frame_id; 357 | std::string test_base_frame_id = odometry_message.child_frame_id; 358 | /* tf_frame_prefix_enable is false so no modifications to the frame id's */ 359 | ASSERT_EQ(test_odom_frame_id, odom_id); 360 | ASSERT_EQ(test_base_frame_id, base_link_id); 361 | } 362 | 363 | TEST_F(TestOmniWheelController, configure_succeeds_tf_test_prefix_true_set_namespace) 364 | { 365 | std::string test_namespace = "/test_namespace"; 366 | 367 | const auto ret = controller_->init(controller_name, "", 10, test_namespace, 368 | rclcpp::NodeOptions() 369 | .allow_undeclared_parameters(true) 370 | .automatically_declare_parameters_from_overrides(true)); 371 | ASSERT_EQ(ret, controller_interface::return_type::OK); 372 | 373 | std::string odom_id = "odom"; 374 | std::string base_link_id = "base_link"; 375 | std::string frame_prefix = "test_prefix"; 376 | 377 | controller_->get_node()->set_parameter( 378 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 379 | 380 | controller_->get_node()->set_parameter( 381 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); 382 | controller_->get_node()->set_parameter( 383 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 384 | controller_->get_node()->set_parameter( 385 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 386 | controller_->get_node()->set_parameter( 387 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 388 | 389 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 390 | 391 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 392 | std::string test_odom_frame_id = odometry_message.header.frame_id; 393 | std::string test_base_frame_id = odometry_message.child_frame_id; 394 | 395 | /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame 396 | * id's instead of the namespace*/ 397 | ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); 398 | ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); 399 | } 400 | 401 | TEST_F(TestOmniWheelController, configure_succeeds_tf_blank_prefix_true_set_namespace) 402 | { 403 | std::string test_namespace = "/test_namespace"; 404 | 405 | const auto ret = controller_->init(controller_name, "", 10, test_namespace, 406 | rclcpp::NodeOptions() 407 | .allow_undeclared_parameters(true) 408 | .automatically_declare_parameters_from_overrides(true)); 409 | ASSERT_EQ(ret, controller_interface::return_type::OK); 410 | 411 | std::string odom_id = "odom"; 412 | std::string base_link_id = "base_link"; 413 | std::string frame_prefix = ""; 414 | 415 | controller_->get_node()->set_parameter( 416 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 417 | 418 | controller_->get_node()->set_parameter( 419 | rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); 420 | controller_->get_node()->set_parameter( 421 | rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); 422 | controller_->get_node()->set_parameter( 423 | rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); 424 | controller_->get_node()->set_parameter( 425 | rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); 426 | 427 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 428 | 429 | auto odometry_message = controller_->get_rt_odom_publisher()->msg_; 430 | std::string test_odom_frame_id = odometry_message.header.frame_id; 431 | std::string test_base_frame_id = odometry_message.child_frame_id; 432 | /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the 433 | * frame id's */ 434 | ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); 435 | ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); 436 | } 437 | 438 | TEST_F(TestOmniWheelController, activate_fails_without_resources_assigned) 439 | { 440 | const auto ret = controller_->init(controller_name, "", 10, "", 441 | rclcpp::NodeOptions() 442 | .allow_undeclared_parameters(true) 443 | .automatically_declare_parameters_from_overrides(true)); 444 | ASSERT_EQ(ret, controller_interface::return_type::OK); 445 | 446 | controller_->get_node()->set_parameter( 447 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 448 | 449 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 450 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); 451 | } 452 | 453 | TEST_F(TestOmniWheelController, activate_succeeds_with_pos_resources_assigned) 454 | { 455 | const auto ret = controller_->init(controller_name, "", 10, "", 456 | rclcpp::NodeOptions() 457 | .allow_undeclared_parameters(true) 458 | .automatically_declare_parameters_from_overrides(true)); 459 | ASSERT_EQ(ret, controller_interface::return_type::OK); 460 | 461 | // We implicitly test that by default position feedback is required 462 | controller_->get_node()->set_parameter( 463 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 464 | 465 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 466 | assignResourcesPosFeedback(); 467 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); 468 | } 469 | 470 | TEST_F(TestOmniWheelController, cleanup) 471 | { 472 | const auto ret = controller_->init(controller_name, "", 10, "", 473 | rclcpp::NodeOptions() 474 | .allow_undeclared_parameters(true) 475 | .automatically_declare_parameters_from_overrides(true)); 476 | ASSERT_EQ(ret, controller_interface::return_type::OK); 477 | 478 | controller_->get_node()->set_parameter( 479 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 480 | controller_->get_node()->set_parameter( 481 | rclcpp::Parameter("omni_wheel_angle", rclcpp::ParameterValue(omni_wheel_angle))); 482 | controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); 483 | controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); 484 | 485 | rclcpp::executors::SingleThreadedExecutor executor; 486 | executor.add_node(controller_->get_node()->get_node_base_interface()); 487 | auto state = controller_->get_node()->configure(); 488 | ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); 489 | assignResourcesPosFeedback(); 490 | 491 | state = controller_->get_node()->activate(); 492 | ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); 493 | 494 | waitForSetup(); 495 | 496 | // send msg 497 | const double linear = 1.0; 498 | const double angular = 1.0; 499 | publish(linear, angular); 500 | controller_->wait_for_twist(executor); 501 | 502 | ASSERT_EQ( 503 | controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), 504 | controller_interface::return_type::OK); 505 | 506 | state = controller_->get_node()->deactivate(); 507 | ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); 508 | ASSERT_EQ( 509 | controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), 510 | controller_interface::return_type::OK); 511 | 512 | state = controller_->get_node()->cleanup(); 513 | ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); 514 | 515 | // should be stopped 516 | EXPECT_EQ(0.0, wheel0_wheel_vel_cmd_.get_value()); 517 | EXPECT_EQ(0.0, wheel1_wheel_vel_cmd_.get_value()); 518 | EXPECT_EQ(0.0, wheel2_wheel_vel_cmd_.get_value()); 519 | EXPECT_EQ(0.0, wheel3_wheel_vel_cmd_.get_value()); 520 | 521 | executor.cancel(); 522 | } 523 | 524 | TEST_F(TestOmniWheelController, correct_initialization_using_parameters) 525 | { 526 | const auto ret = controller_->init(controller_name, "", 10, "", 527 | rclcpp::NodeOptions() 528 | .allow_undeclared_parameters(true) 529 | .automatically_declare_parameters_from_overrides(true)); 530 | ASSERT_EQ(ret, controller_interface::return_type::OK); 531 | 532 | controller_->get_node()->set_parameter( 533 | rclcpp::Parameter("omni_wheel_names", rclcpp::ParameterValue(omni_wheel_names))); 534 | controller_->get_node()->set_parameter( 535 | rclcpp::Parameter("omni_wheel_angle", rclcpp::ParameterValue(omni_wheel_angle))); 536 | controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); 537 | controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); 538 | 539 | rclcpp::executors::SingleThreadedExecutor executor; 540 | executor.add_node(controller_->get_node()->get_node_base_interface()); 541 | 542 | auto state = controller_->get_node()->configure(); 543 | assignResourcesPosFeedback(); 544 | 545 | ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); 546 | EXPECT_EQ(0.01, wheel0_wheel_vel_cmd_.get_value()); 547 | EXPECT_EQ(0.02, wheel1_wheel_vel_cmd_.get_value()); 548 | EXPECT_EQ(0.03, wheel2_wheel_vel_cmd_.get_value()); 549 | EXPECT_EQ(0.04, wheel3_wheel_vel_cmd_.get_value()); 550 | 551 | state = controller_->get_node()->activate(); 552 | ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); 553 | 554 | // send msg 555 | const double linear = 1.0; 556 | const double angular = 0.0; 557 | publish(linear, angular); 558 | // wait for msg is be published to the system 559 | ASSERT_TRUE(controller_->wait_for_twist(executor)); 560 | 561 | ASSERT_EQ( 562 | controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), 563 | controller_interface::return_type::OK); 564 | EXPECT_NEAR(0.707107, wheel0_wheel_vel_cmd_.get_value(), 0.0001); 565 | EXPECT_NEAR(0.707107, wheel1_wheel_vel_cmd_.get_value(), 0.0001); 566 | EXPECT_NEAR(-0.707107, wheel2_wheel_vel_cmd_.get_value(), 0.0001); 567 | EXPECT_NEAR(-0.707107, wheel3_wheel_vel_cmd_.get_value(), 0.0001); 568 | 569 | // deactivated 570 | // wait so controller process the second point when deactivated 571 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 572 | state = controller_->get_node()->deactivate(); 573 | ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); 574 | ASSERT_EQ( 575 | controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), 576 | controller_interface::return_type::OK); 577 | 578 | EXPECT_EQ(0.0, wheel0_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; 579 | EXPECT_EQ(0.0, wheel1_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; 580 | EXPECT_EQ(0.0, wheel2_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; 581 | EXPECT_EQ(0.0, wheel3_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; 582 | 583 | // cleanup 584 | state = controller_->get_node()->cleanup(); 585 | ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); 586 | EXPECT_EQ(0.0, wheel0_wheel_vel_cmd_.get_value()); 587 | EXPECT_EQ(0.0, wheel1_wheel_vel_cmd_.get_value()); 588 | EXPECT_EQ(0.0, wheel2_wheel_vel_cmd_.get_value()); 589 | EXPECT_EQ(0.0, wheel3_wheel_vel_cmd_.get_value()); 590 | 591 | state = controller_->get_node()->configure(); 592 | ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); 593 | executor.cancel(); 594 | } 595 | --------------------------------------------------------------------------------