diff --git a/omnidrive_rppico/hardware/omnibot_pico_system.cpp b/omnidrive_rppico/hardware/omnibot_pico_system.cpp index d0ad7bf..9de356f 100644 --- a/omnidrive_rppico/hardware/omnibot_pico_system.cpp +++ b/omnidrive_rppico/hardware/omnibot_pico_system.cpp @@ -255,7 +255,7 @@ hardware_interface::return_type omnidrive_rppico ::OmniDriveRpPicoHardware::writ int motor_r_counts_per_loop = wheel_r_.cmd / wheel_r_.rads_per_count / cfg_.loop_rate; - + comms_.set_motor_values(motor_f_counts_per_loop, motor_l_counts_per_loop, motor_b_counts_per_loop, diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml new file mode 100644 index 0000000..2ba415b --- /dev/null +++ b/src/ezbot_robot/config/omnidirectional_controller.yaml @@ -0,0 +1,35 @@ +controller_manager: + ros__parameters: + update_rate: 30 + #use_sim_time: true + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + omnidirectional_controller: + type: omnidirectional_controllers/OmnidirectionalController + +omnidirectional_controller: + ros__parameters: + wheel_names: + - roue_avant_joint + - roue_gauche_joint + - roue_arriere_joint + - roue_droite_joint + + robot_radius: 0.1 + wheel_radius: 0.053112205 + gamma: 30.0 # angle between body fixed y axis and the normal of wheel3 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + odom_numeric_integration_method: runge_kutta2 + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + use_stamped_vel: false \ No newline at end of file diff --git a/src/ezbot_robot/config/ros2_controllers.yaml b/src/ezbot_robot/config/ros2_controllers.yaml new file mode 100644 index 0000000..c4fd749 --- /dev/null +++ b/src/ezbot_robot/config/ros2_controllers.yaml @@ -0,0 +1,19 @@ +controller_manager: + ros__parameters: + update_rate: 100 + use_sim_time: true + + omnidirectional_controller: + type: forward_command_controller/ForwardCommandController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +omnidirectional_controller: + ros__parameters: + joints: + - roue_avant_joint + - roue_gauche_joint + - roue_arriere_joint + - roue_droite_joint + interface_name: velocity \ No newline at end of file diff --git a/src/ezbot_robot/description/robot_core.xacro b/src/ezbot_robot/description/robot_core.xacro index e7e9e5d..ec6fd7f 100644 --- a/src/ezbot_robot/description/robot_core.xacro +++ b/src/ezbot_robot/description/robot_core.xacro @@ -257,7 +257,7 @@ - 0clear + 0 0.02 diff --git a/src/ezbot_robot/description/ros2_control.xacro b/src/ezbot_robot/description/ros2_control.xacro index 9ef8cf0..8ca2360 100644 --- a/src/ezbot_robot/description/ros2_control.xacro +++ b/src/ezbot_robot/description/ros2_control.xacro @@ -12,7 +12,7 @@ roue_droite_joint 30 - /dev/ttyAMA1 + /dev/pts/11 115200 10000 diff --git a/src/ezbot_robot/launch/real_robot.launch.py b/src/ezbot_robot/launch/real_robot.launch.py index 386de8f..2450da4 100644 --- a/src/ezbot_robot/launch/real_robot.launch.py +++ b/src/ezbot_robot/launch/real_robot.launch.py @@ -45,13 +45,10 @@ def generate_launch_description(): ) #robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description']) - controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'controllers.yaml') - - pkg_path = os.path.join(get_package_share_directory('ezbot_robot')) - xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro') - robot_description = {"robot_description": xacro.process_file(xacro_file).toxml()} - + #controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'controllers.yaml') + controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'omnidirectional_controller.yaml') + controller_manager = Node( package="controller_manager", executable="ros2_control_node", @@ -75,7 +72,7 @@ def generate_launch_description(): arguments=['omnidirectional_controller'], ) delayed_omnidrive_spawner = TimerAction( - period=1.0, + period=10.0, actions=[omnidrive_spawner], ) diff --git a/src/omnidirectional_controllers/.gitattributes b/src/omnidirectional_controllers/.gitattributes new file mode 100644 index 0000000..24a8e87 --- /dev/null +++ b/src/omnidirectional_controllers/.gitattributes @@ -0,0 +1 @@ +*.png filter=lfs diff=lfs merge=lfs -text diff --git a/src/omnidirectional_controllers/.github/workflows/humble-source-build.yaml b/src/omnidirectional_controllers/.github/workflows/humble-source-build.yaml new file mode 100644 index 0000000..de65c08 --- /dev/null +++ b/src/omnidirectional_controllers/.github/workflows/humble-source-build.yaml @@ -0,0 +1,20 @@ +name: Humble Source Build + +on: + pull_request: + branches: + - humble + +jobs: + humble_source_build: + runs-on: ubuntu-22.04 + steps: + - uses: ros-tooling/setup-ros@v0.7 + with: + rosdistro: humble + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: omnidirectional_controllers + ref: ${{ github.event.pull_request.head.sha }} + target-ros2-distro: humble + skip-tests: true diff --git a/src/omnidirectional_controllers/.gitignore b/src/omnidirectional_controllers/.gitignore new file mode 100644 index 0000000..5faba10 --- /dev/null +++ b/src/omnidirectional_controllers/.gitignore @@ -0,0 +1,55 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + + +# vscode folder +.vscode diff --git a/src/omnidirectional_controllers/CMakeLists.txt b/src/omnidirectional_controllers/CMakeLists.txt new file mode 100644 index 0000000..73ed759 --- /dev/null +++ b/src/omnidirectional_controllers/CMakeLists.txt @@ -0,0 +1,139 @@ +cmake_minimum_required(VERSION 3.5) +project(omnidirectional_controllers) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp_lifecycle + rclcpp + tf2_msgs + tf2_ros + tf2 +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(odometry SHARED) + +target_sources(odometry + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/odometry.cpp +) + +target_include_directories(odometry + PUBLIC $ + $ +) + +add_library(kinematics SHARED) + +target_sources(kinematics + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics.cpp +) + +target_include_directories(kinematics + PUBLIC $ + $ +) + +add_library(${PROJECT_NAME} SHARED + src/omnidirectional_controller.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +target_link_libraries(${PROJECT_NAME} + odometry + kinematics +) + +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(controller_interface omnidirectional_plugin.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + + ament_lint_auto_find_test_dependencies() + + ament_add_gmock( + test_odometry + test/test_odometry.cpp + src/odometry.cpp + ) + + target_link_libraries(test_odometry + odometry + kinematics + ) + + ament_add_gmock( + test_kinematics + test/test_kinematics.cpp + src/kinematics.cpp + ) + + target_link_libraries(test_kinematics + odometry + kinematics + ) + + target_include_directories(test_odometry PRIVATE include) + ament_target_dependencies(test_odometry) + + target_include_directories(test_kinematics PRIVATE include) + ament_target_dependencies(test_kinematics) +endif() + +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_package() diff --git a/src/omnidirectional_controllers/LICENSE b/src/omnidirectional_controllers/LICENSE new file mode 100644 index 0000000..2b7f10e --- /dev/null +++ b/src/omnidirectional_controllers/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2022 Mateus Menezes + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/omnidirectional_controllers/README.md b/src/omnidirectional_controllers/README.md new file mode 100644 index 0000000..49d2486 --- /dev/null +++ b/src/omnidirectional_controllers/README.md @@ -0,0 +1,104 @@ +# Omnidirectional Controllers + +This package provides ROS 2 controllers for controlling Omnidirectional robots with three wheels. It is based on the concepts of [ros2_control] and [ros2_controllers]. Initially, only forward and inverse kinematics, based solely on the [diff_drive_controller], and odometry calculation have been implemented. The input for control is robot body velocity ($\dot{x}$, $\dot{y}$, $\dot{\theta}$) commands, which are translated into wheel commands ($\omega_1$, $\omega_2$, $\omega_3$) for an omnidirectional robot with three wheels. Odometry is computed from hardware feedback and published. It is worth noting that there are plans to further develop advanced linear and non-linear controllers, such as Model Predictive Control (MPC). + +**Author:** Mateus Menezes
+**Maintainer:** Mateus Menezes, mateusmenezes95@gmail.com + +## Build status + +ROS2 Distro | Branch | Build status | +:---------: | :----: | :----------: | +**Humble** | [`humble`](https://github.com/mateusmenezes95/omnidirectional_controllers/tree/humble) | [![Build From Source](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml/badge.svg)](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml) + +## Installation Premises + +1. This repository has been tested on [ROS2 Foxy] and with [Classic Gazebo 11]; + +2. These instructions assume that you have already installed ROS2 Foxy Fitzroy on your machine. If not, please follow the recommended [recommended ubuntu installation tutorial]; + +3. Before installing the package, you will need to have an ament workspace set up. If you don't have one, follow the instructions in the [Creating a workspace tutorial]. Once you have created the workspace, clone this repository in the source folder of your workspace. + +## Installation + +> **ATTENTION:** These commands assume that you have created a workspace called "ros_ws" in your home folder. If you used a different directory or name, please adjust the commands accordingly. + +After installing ROS2 and creating the workspace, clone this repository in your workspace: + +``` +cd ~/ros_ws/src +git clone https://github.com/mateusmenezes95/omnidirectional_controllers +``` + +Install the binary dependencies by running the following command in the root of your workspace: + +``` +cd ~/ros_ws +rosdep init +rosdep update +sudo apt update +rosdep install --from-paths src/omnidirectional_controllers --ignore-src -r -y --rosdistro foxy +``` + +If all dependencies are already installed, you should see the message "All required rosdeps installed successfully." + +## Building + +Run the following command to build the package: + +``` +cd ~/ros_ws +colcon build --symlink-install --event-handlers console_direct+ +``` + +> Run `colcon build --help` to understand the arguments passed! + +After building the package, open a new terminal and navigate to your workspace. Then, source the overlay by running the following command: + +``` +source /opt/ros/foxy/setup.bash +cd ~/ros_ws +. install/local_setup.bash +``` + +> See [Source the overlay] to learn about underlay and overlay concepts. + +## Usage + +You must follow the three steps explained in [Running the Framework for Your Robot](https://control.ros.org/master/doc/getting_started/getting_started.html#running-the-framework-for-your-robot) tutorial. + +For an concrete example of how to use the Omnidirectional controllers, refer to the Axebot simulation's [controller configuration], [ros2_control URDF], and [launch file]. + +### Subscribed Topic + +* **`/omnidirectional_controller/cmd_vel_unstamped`** ([geometry_msgs/msg/Twist]) + + Velocity twist from which the controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +### Published Topic + +* **`/omnidirectional_controller/odom`** ([nav_msgs/msg/Odometry]) + + Robot odometry. The odometry can be computed from hardware feedback or using an open-loop approach, in which the integration is performed using the Twist command. You can select the approach in the configuration file. Additionally, you can choose either the Runge-Kutta or Euler Forward integration method. + +## Unit Test + +> TODO + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker] + +[ros2_control]: https://control.ros.org/master/index.html +[ros2_controllers]: https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html +[Issue Tracker]: https://github.com/mateusmenezes95/omnidirectional_controllers/issues +[diff_drive_controller]: https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html +[ros2_control URDF]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_description/urdf/ros2_control.urdf.xacro +[controller configuration]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_control/config/omnidirectional_controller.yaml +[launch file]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_gazebo/launch/axebot.launch.py +[Creating a workspace tutorial]: https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html#creating-a-workspace +[recommended ubuntu installation tutorial]: https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html +[Source the overlay]: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html#source-the-overlay +[geometry_msgs/msg/Twist]: https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html +[nav_msgs/msg/Odometry]: https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html +[Classic Gazebo 11]: https://classic.gazebosim.org/ diff --git a/src/omnidirectional_controllers/doc/images/.gitattributes b/src/omnidirectional_controllers/doc/images/.gitattributes new file mode 100644 index 0000000..4fae6dc --- /dev/null +++ b/src/omnidirectional_controllers/doc/images/.gitattributes @@ -0,0 +1 @@ +*.jpg filter=lfs diff=lfs merge=lfs -text diff --git a/src/omnidirectional_controllers/doc/kinematics_and_odometry.md b/src/omnidirectional_controllers/doc/kinematics_and_odometry.md new file mode 100644 index 0000000..b0672fc --- /dev/null +++ b/src/omnidirectional_controllers/doc/kinematics_and_odometry.md @@ -0,0 +1,218 @@ +# Omnidirectional Robot Kinematics and Odometry + +- [Kinematics](#kinematics) + - [Inverse](#inverse) + - [Forward](#forward) +- [Odometry](#odometry) +- [References](#references) + +## Kinematics + +The figure below describe the geometry paramaters used in the forward and inverse kinematics implemented on this controller. + +

+ +
+ Source: [1] +

+ +### Inverse + +From the geometry analysis in the above figure, we have that: + +```math +\begin{align} + v_{m_1} &= -v_n + \omega L\\ + v_{m_2} &=v\cos(\gamma) + v_n\sin(\gamma) + \omega L\\ + v_{m_3} &=-v\cos(\gamma) + v_n\sin(\gamma) + \omega L +\end{align} +``` + +Or in Matrix form: + +```math +\begin{equation} + \begin{bmatrix} + v_{m_1}\\ + v_{m_2}\\ + v_{m_3} + \end{bmatrix}= + \begin{bmatrix} + 0 & -1 & L\\ + \cos(\gamma) & \sin(\gamma) & L\\ + -\cos(\gamma) & \sin(\gamma) & L\\ + \end{bmatrix} + \begin{bmatrix} + v\\ + v_{n}\\ + \omega + \end{bmatrix} +\end{equation} +``` + +For the wheels’ angular velocities, assuming that all wheels have the same radius $r$ + +```math +\begin{equation} + \begin{bmatrix} + \omega_{m_1}\\ + \omega_{m_2}\\ + \omega_{m_3} + \end{bmatrix} + = + \frac{1}{r} + \begin{bmatrix} + 0 & -1 & L\\ + \cos(\gamma) & \sin(\gamma) & L\\ + -\cos(\gamma) & \sin(\gamma) & L\\ + \end{bmatrix} + \begin{bmatrix} + v\\ + v_{n}\\ + \omega + \end{bmatrix} +\end{equation} +``` + +### Forward + +Manipulating the inverse kinematics, we get in + +```math +\begin{equation} + \begin{bmatrix} + v\\ + v_{n}\\ + \omega + \end{bmatrix}=r + \begin{bmatrix} + 0&\frac{1}{2\cos(\gamma)}&\frac{-1}{2\cos(\gamma)}\\ + \frac{-1}{\sin(\gamma)+1}&\frac{1}{2(\sin(\gamma)+1)}&\frac{1}{2(\sin(\gamma)+1)}\\ + \frac{\sin(\gamma)}{L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)} + \end{bmatrix} + \begin{bmatrix} + \omega_{m_1}\\ + \omega_{m_2}\\ + \omega_{m_3} + \end{bmatrix} +\end{equation} +``` + +Or + +```math +\begin{equation} + \begin{bmatrix} + v\\ + v_{n}\\ + \omega + \end{bmatrix} + = + rG^{-1} + \begin{bmatrix} + \omega_{m_1}\\ + \omega_{m_2}\\ + \omega_{m_3} + \end{bmatrix} +\end{equation} +``` + +Whereby + +```math +\begin{equation} + G^{-1} = + \begin{bmatrix} + 0&\frac{1}{2\cos(\gamma)}&\frac{-1}{2\cos(\gamma)}\\ + \frac{-1}{\sin(\gamma)+1}&\frac{1}{2(\sin(\gamma)+1)}&\frac{1}{2(\sin(\gamma)+1)}\\ + \frac{\sin(\gamma)}{L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)}&\frac{1}{2L(\sin(\gamma)+1)} + \end{bmatrix} +\end{equation} +``` + +Given that $\beta = \frac{1}{2\cos(\gamma)}$ and $\alpha = \frac{1}{\sin(\gamma)+1}$, we have + +```math +\begin{equation} + G^{-1}= + \begin{bmatrix} + 0 & \beta & -\beta\\ + -\alpha & 0.5\alpha & 0.5\alpha\\ + L^{-1}\sin(\gamma)\alpha & 0.5L^{-1}\alpha & 0.5L^{-1}\alpha + \end{bmatrix} +\end{equation} +``` + +Therefore + +```math +\begin{align} + v &= \beta(\omega_{m_2} - \omega_{m_3})\\ + v_n &=\alpha(-w_{m_1} +0.5(w_{m_2}+w_{m_3}))\\ + \omega &= L^{-1}\alpha(\sin(\gamma)w_{m_1}+0.5(w_{m_2}+w_{m_3})) +\end{align} +``` + +## Odometry + +The robot’s velocity with regard to the world (w.r.t) is given by + +```math +\begin{equation} + \begin{bmatrix} + \dot{x}_r\\ + \dot{y}_r\\ + \dot{\theta} + \end{bmatrix} + = + R(\theta) + \begin{bmatrix} + v\\ + v_{n}\\ + \omega + \end{bmatrix}1 +\end{equation} +``` + +where $R(\theta)$ is the rotation matrix around the $z$ axis perpendicular de horizontal plane $xy$, given by + +```math +\begin{equation} + R(\theta) + = + \begin{bmatrix} + \cos(\theta) & -\sin(\theta) & 0\\ + \sin(\theta) & \cos(\theta) & 0\\ + 0 & 0 & 1 + \end{bmatrix} +\end{equation} +``` + +Assume that the robot configuration $\mathbf{q}(t_k) = \mathbf{q}_k = (x_k,y_k,\theta_k)$ at the sampling time $t_k$ is known, together with the velocity inputs $v_k$, $v_{n_k}$, and $w_k$ applied in the interval $[t_k,\:t_{k+1})$. The value of the configuration variable $\mathbf{q}_{k+1}$ at the sampling time $t_{k+1}$ can then be reconstructed by forward integration of the kinematic model. Adapted from [2]. + +Considering that the sampling period $T_s = t_{k+1} - t_k$, the Euler method for the axebot configuration $\mathbf{q}_{k+1}$ is given by + +```math +\begin{align} + x_{k+1} &= x_k + (v\cos(\theta_k)-v_n\sin(\theta_k))T_s\\ + y_{k+1} &= y_k + (v\sin(\theta_k)+v_n\cos(\theta_k))T_s\\ + \theta_{k+1} &= \theta_k + \omega_kT_s +\end{align} +``` + +And for the second-order Runge Kutta integration method + +```math +\begin{align} + \bar{\theta}_k &= \theta_k+\frac{w_kTs}{2}\\ + x_{k+1} &= x_k + (v\cos(\bar{\theta}_k)-v_n\sin(\bar{\theta}_k))T_s\\ + y_{k+1} &= y_k + (v\sin(\bar{\theta}_k)+v_n\cos(\bar{\theta}_k))T_s\\ + \theta_{k+1} &= \theta_k + \omega T_s +\end{align} +``` + +## References + +[1] J. Santos, A. Conceição, T. Santos, and H. Ara ujo, “Remote control of an omnidirectional mobile robot with time-varying delay and noise attenuation,” *Mechatronics*, vol. 52, pp. 7–21, 2018, ISSN: 0957-4158. DOI: 10.1016/j.mechatronics.2018.04.003. [Online](https://www.sciencedirect.com/science/article/abs/pii/S0957415818300606). + +[2] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, *Robotics: Modelling, Planning and Control*, 1st ed. Springer Publishing Company, Incorporated, 2008. diff --git a/src/omnidirectional_controllers/include/omnidirectional_controllers/kinematics.hpp b/src/omnidirectional_controllers/include/omnidirectional_controllers/kinematics.hpp new file mode 100644 index 0000000..fa60ea1 --- /dev/null +++ b/src/omnidirectional_controllers/include/omnidirectional_controllers/kinematics.hpp @@ -0,0 +1,57 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ +#define OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ + +#include +#include + +#include "omnidirectional_controllers/types.hpp" + +namespace omnidirectional_controllers { + +constexpr double OMNI_ROBOT_MAX_WHEELS = 4; + +class Kinematics { + public: + explicit Kinematics(RobotParams robot_params); + Kinematics(); + ~Kinematics(); + // Forward kinematics + RobotVelocity getBodyVelocity(const std::vector & wheels_vel); + // Inverse kinematics + std::vector getWheelsAngularVelocities(RobotVelocity vel); + void setRobotParams(RobotParams robot_params); + private: + void initializeParams(); + RobotParams robot_params_; + std::vector angular_vel_vec_; + double cos_gamma_; + double sin_gamma_; + double alpha_; + double beta_; +}; + +} // namespace omnidirectional_controllers + +#endif // OMNIDIRECTIONAL_CONTROLLERS__KINEMATICS_HPP_ diff --git a/src/omnidirectional_controllers/include/omnidirectional_controllers/odometry.hpp b/src/omnidirectional_controllers/include/omnidirectional_controllers/odometry.hpp new file mode 100644 index 0000000..08e10e9 --- /dev/null +++ b/src/omnidirectional_controllers/include/omnidirectional_controllers/odometry.hpp @@ -0,0 +1,66 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ +#define OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ + +#include +#include + +#include "omnidirectional_controllers/kinematics.hpp" +#include "omnidirectional_controllers/types.hpp" + +namespace omnidirectional_controllers { + +constexpr char EULER_FORWARD[] = "euler_forward"; +constexpr char RUNGE_KUTTA2[] = "runge_kutta2"; + +class Odometry { + public: + Odometry(); + ~Odometry(); + bool setNumericIntegrationMethod(const std::string & numeric_integration_method); + void setRobotParams(RobotParams params); + void updateOpenLoop(RobotVelocity vel, double dt); + void update(const std::vector & wheels_vel, double dt); + RobotPose getPose() const; + RobotVelocity getBodyVelocity() const; + void reset(); + + protected: + void integrateByRungeKutta(); + void integrateByEuler(); + void integrateVelocities(); + RobotVelocity body_vel_{0, 0, 0}; + double dt_; + + private: + RobotPose pose_{0, 0, 0}; + RobotParams robot_params_{0, 0, 0}; + std::string numeric_integration_method_ = EULER_FORWARD; + Kinematics robot_kinematics_; + bool is_robot_param_set_{false}; +}; + +} // namespace omnidirectional_controllers + +#endif // OMNIDIRECTIONAL_CONTROLLERS__ODOMETRY_HPP_ diff --git a/src/omnidirectional_controllers/include/omnidirectional_controllers/omnidirectional_controller.hpp b/src/omnidirectional_controllers/include/omnidirectional_controllers/omnidirectional_controller.hpp new file mode 100644 index 0000000..5744182 --- /dev/null +++ b/src/omnidirectional_controllers/include/omnidirectional_controllers/omnidirectional_controller.hpp @@ -0,0 +1,119 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ +#define OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include "omnidirectional_controllers/kinematics.hpp" +#include "omnidirectional_controllers/odometry.hpp" +#include "omnidirectional_controllers/types.hpp" + +namespace omnidirectional_controllers { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class OmnidirectionalController : public controller_interface::ControllerInterface { + public: + OmnidirectionalController(); + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + CallbackReturn on_init() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + ~OmnidirectionalController(); + + protected: + struct WheelHandle { + std::reference_wrapper velocity_state; + std::reference_wrapper velocity_command; + }; + + std::vector wheel_names_; + std::vector registered_wheel_handles_; + + // Default parameters for axebot + RobotParams robot_params_{0.1, 0.0505, 0.0}; + + struct OdometryParams { + bool open_loop = false; + bool enable_odom_tf = true; + std::string base_frame_id = "base_link"; + std::string odom_frame_id = "odom"; + std::string odom_numeric_integration_method = EULER_FORWARD; + std::array pose_covariance_diagonal; + std::array twist_covariance_diagonal; + } odom_params_; + + bool use_stamped_vel_ = true; + + Kinematics omni_robot_kinematics_; + Odometry odometry_; + + rclcpp::Publisher::SharedPtr odometry_publisher_ = nullptr; + rclcpp::Publisher::SharedPtr odometry_transform_publisher_ = nullptr; + + tf2_msgs::msg::TFMessage odometry_transform_message_; + nav_msgs::msg::Odometry odometry_message_; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr vel_cmd_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + vel_cmd_unstamped_subscriber_ = nullptr; + + rclcpp::Time previous_update_timestamp_{0}; + + double publish_rate_{50.0}; + rclcpp::Duration publish_period_{0, 0}; + rclcpp::Time previous_publish_timestamp_{0}; + + private: + void velocityCommandStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel); + void velocityCommandUnstampedCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel); + geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel_; + double cos_gamma{0}; + double sin_gamma{0}; + + std::shared_ptr node_; +}; + +} // namespace omnidirectional_controllers + +#endif // OMNIDIRECTIONAL_CONTROLLERS__OMNIDIRECTIONAL_CONTROLLER_HPP_ diff --git a/src/omnidirectional_controllers/include/omnidirectional_controllers/types.hpp b/src/omnidirectional_controllers/include/omnidirectional_controllers/types.hpp new file mode 100644 index 0000000..234131c --- /dev/null +++ b/src/omnidirectional_controllers/include/omnidirectional_controllers/types.hpp @@ -0,0 +1,50 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ +#define OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ + +#define DEG2RAD(deg) (deg * M_PI / 180.0) + +namespace omnidirectional_controllers { + +struct RobotParams { + double wheel_radius; + double robot_radius; + double gamma; +}; + +struct RobotVelocity { + double vx; // [m/s] + double vy; // [m/s] + double omega; // [rad] +}; + +struct RobotPose { + double x; // [m] + double y; // [m] + double theta; // [rad] +}; + +} // namespace omnidirectional_controllers + +#endif // OMNIDIRECTIONAL_CONTROLLERS__TYPES_HPP_ diff --git a/src/omnidirectional_controllers/omnidirectional_plugin.xml b/src/omnidirectional_controllers/omnidirectional_plugin.xml new file mode 100644 index 0000000..1cb2a33 --- /dev/null +++ b/src/omnidirectional_controllers/omnidirectional_plugin.xml @@ -0,0 +1,7 @@ + + + + The Omnidirectional controller transforms linear and angular velocity messages into signals for each wheel(s) for a Omnidirectional robot with four wheels. + + + diff --git a/src/omnidirectional_controllers/package.xml b/src/omnidirectional_controllers/package.xml new file mode 100644 index 0000000..dc5bcea --- /dev/null +++ b/src/omnidirectional_controllers/package.xml @@ -0,0 +1,33 @@ + + + + omnidirectional_controllers + 0.0.0 + Controller for a omnidirectional robot with four wheels + Vincent Belpois + MIT + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp_lifecycle + rclcpp + tf2_msgs + tf2_ros + tf2 + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/src/omnidirectional_controllers/src/kinematics.cpp b/src/omnidirectional_controllers/src/kinematics.cpp new file mode 100644 index 0000000..058e830 --- /dev/null +++ b/src/omnidirectional_controllers/src/kinematics.cpp @@ -0,0 +1,84 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include +#include + +#include "omnidirectional_controllers/kinematics.hpp" + +#include "omnidirectional_controllers/types.hpp" + +namespace omnidirectional_controllers { + +Kinematics::Kinematics(RobotParams robot_params) + : robot_params_(robot_params) { + this->initializeParams(); +} + +Kinematics::Kinematics() { + this->initializeParams(); +} + +RobotVelocity Kinematics::getBodyVelocity(const std::vector & wheels_vel) { + RobotVelocity vel; + double wm0 = wheels_vel.at(0); + double wm1 = wheels_vel.at(1); + double wm2 = wheels_vel.at(2); + double wm3 = wheels_vel.at(3); + + vel.vx = (wm3 - wm1)* robot_params_.wheel_radius; + + vel.vy = (wm0 - wm2) * robot_params_.wheel_radius; + + vel.omega = (1/robot_params_.robot_radius) * (wm0 + wm1 + wm2 + wm3) + * robot_params_.wheel_radius; + + return vel; +} + +std::vector Kinematics::getWheelsAngularVelocities(RobotVelocity vel) { + double vx = vel.vx; + double vy = vel.vy; + double wl = vel.omega * robot_params_.robot_radius; + + angular_vel_vec_[0] = (wl + vy) / robot_params_.wheel_radius; + angular_vel_vec_[1] = (wl - vx) / robot_params_.wheel_radius; + angular_vel_vec_[2] = (wl - vy) / robot_params_.wheel_radius; + angular_vel_vec_[3] = (wl + vx) / robot_params_.wheel_radius; + + return angular_vel_vec_; +} + +void Kinematics::setRobotParams(RobotParams robot_params) { + this->robot_params_ = robot_params; + this->initializeParams(); +} + +void Kinematics::initializeParams() { + angular_vel_vec_.reserve(OMNI_ROBOT_MAX_WHEELS); + angular_vel_vec_ = {0, 0, 0, 0}; + // removed gamma (in our case, x_robot ) + } + +Kinematics::~Kinematics() {} + +} // namespace omnidirectional_controllers diff --git a/src/omnidirectional_controllers/src/odometry.cpp b/src/omnidirectional_controllers/src/odometry.cpp new file mode 100644 index 0000000..0f63012 --- /dev/null +++ b/src/omnidirectional_controllers/src/odometry.cpp @@ -0,0 +1,118 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "omnidirectional_controllers/odometry.hpp" + +#include +#include +#include +#include +#include + +#include "omnidirectional_controllers/types.hpp" + +namespace omnidirectional_controllers { + +Odometry::Odometry() {} + +bool Odometry::setNumericIntegrationMethod(const std::string & numeric_integration_method) { + if (numeric_integration_method != EULER_FORWARD && + numeric_integration_method != RUNGE_KUTTA2) { + this->numeric_integration_method_ = EULER_FORWARD; + return false; + } + this->numeric_integration_method_ = numeric_integration_method; + return true; +} + +void Odometry::setRobotParams(RobotParams params) { + if (params.wheel_radius < std::numeric_limits::epsilon()) { + std::stringstream error; + error << "Invalid wheel radius " << params.wheel_radius << " set!" << std::endl; + throw std::runtime_error(error.str()); + } + + if (params.robot_radius < std::numeric_limits::epsilon()) { + std::stringstream error; + error << "Invalid robot radius " << params.wheel_radius << " set!" << std::endl; + throw std::runtime_error(error.str()); + } + + this->robot_params_ = params; + robot_kinematics_.setRobotParams(robot_params_); + is_robot_param_set_ = true; +} + +void Odometry::updateOpenLoop(RobotVelocity vel, double dt_) { + this->body_vel_ = vel; + this->dt_ = dt_; + this->integrateVelocities(); +} + +void Odometry::update(const std::vector & wheels_vel, double dt) { + if (!is_robot_param_set_) { + throw std::runtime_error(std::string("Robot parameters was not set or not set properly!")); + } + this->dt_ = dt; + this->body_vel_ = robot_kinematics_.getBodyVelocity(wheels_vel); + this->integrateVelocities(); +} + +RobotPose Odometry::getPose() const { + return pose_; +} + +RobotVelocity Odometry::getBodyVelocity() const { + return body_vel_; +} + +void Odometry::reset() { + pose_ = {0, 0, 0}; +} + +void Odometry::integrateByRungeKutta() { + double theta_bar = pose_.theta + (body_vel_.omega*dt_ / 2); + pose_.x = pose_.x + (body_vel_.vx * cos(theta_bar) - body_vel_.vy * sin(theta_bar)) * dt_; + pose_.y = pose_.y + (body_vel_.vx * sin(theta_bar) + body_vel_.vy * cos(theta_bar)) * dt_; + pose_.theta = pose_.theta + body_vel_.omega*dt_; +} + +void Odometry::integrateByEuler() { + pose_.x = pose_.x + (body_vel_.vx * cos(pose_.theta) - body_vel_.vy * sin(pose_.theta)) * dt_; + pose_.y = pose_.y + (body_vel_.vx * sin(pose_.theta) + body_vel_.vy * cos(pose_.theta)) * dt_; + pose_.theta = pose_.theta + body_vel_.omega*dt_; +} + +void Odometry::integrateVelocities() { + if (numeric_integration_method_ == RUNGE_KUTTA2) { + this->integrateByRungeKutta(); + return; + } + // Euler method is the odometry class default! + this->integrateByEuler(); + this->dt_ = 0; + this->body_vel_ = {0, 0, 0}; +} + +Odometry::~Odometry() {} + +} // namespace omnidirectional_controllers diff --git a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp new file mode 100644 index 0000000..8d17224 --- /dev/null +++ b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp @@ -0,0 +1,415 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "omnidirectional_controllers/omnidirectional_controller.hpp" + +#include // NOLINT +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace { +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto WHEELS_QUANTITY = 4; +} // namespace + +namespace omnidirectional_controllers { + +using namespace std::chrono_literals; // NOLINT +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; +using std::placeholders::_1; + +OmnidirectionalController::OmnidirectionalController() + : controller_interface::ControllerInterface() + , cmd_vel_(std::make_shared()) {} + +CallbackReturn OmnidirectionalController::on_init() { + this->node_ = this->get_node(); + + try { + auto_declare>("wheel_names", std::vector()); + + auto_declare("robot_radius", robot_params_.robot_radius); + auto_declare("wheel_radius", robot_params_.wheel_radius); + auto_declare("gamma", robot_params_.gamma); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare("odom_numeric_integration_method", + odom_params_.odom_numeric_integration_method); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration OmnidirectionalController::command_interface_configuration() const { + std::vector conf_names; + + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration OmnidirectionalController::state_interface_configuration() const { + std::vector conf_names; + for (const auto & joint_name : wheel_names_) { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +CallbackReturn OmnidirectionalController::on_configure( + const rclcpp_lifecycle::State & previous_state) { + auto logger = node_->get_logger(); + + RCLCPP_DEBUG(logger, + "Called on_configure. Previous state was %s", + previous_state.label().c_str()); + + // update parameters + wheel_names_ = node_->get_parameter("wheel_names").as_string_array(); + + if (wheel_names_.size() != WHEELS_QUANTITY) { + RCLCPP_ERROR( + logger, "The number of wheels [%zu] and the required [%d] are different", + wheel_names_.size(), WHEELS_QUANTITY); + return CallbackReturn::ERROR; + } + + if (wheel_names_.empty()) { + RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); + return CallbackReturn::ERROR; + } + + robot_params_.robot_radius = node_->get_parameter("robot_radius").as_double(); + robot_params_.wheel_radius = node_->get_parameter("wheel_radius").as_double(); + robot_params_.gamma = node_->get_parameter("gamma").as_double(); + robot_params_.gamma = DEG2RAD(robot_params_.gamma); + + omni_robot_kinematics_.setRobotParams(robot_params_); + odometry_.setRobotParams(robot_params_); + + odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); + odom_params_.odom_numeric_integration_method = node_->get_parameter( + "odom_numeric_integration_method").as_string(); + + if (odom_params_.odom_numeric_integration_method != EULER_FORWARD && + odom_params_.odom_numeric_integration_method != RUNGE_KUTTA2) { + RCLCPP_WARN(logger, + "Invalid numeric integration got: %s. Using default %s intead", + odom_params_.odom_numeric_integration_method.c_str(), + EULER_FORWARD); + odom_params_.odom_numeric_integration_method = EULER_FORWARD; + } + odometry_.setNumericIntegrationMethod(odom_params_.odom_numeric_integration_method); + + cmd_vel_timeout_ = std::chrono::milliseconds{ + static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; + use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); + + // initialize command subscriber + if (use_stamped_vel_) { + vel_cmd_subscriber_ = node_->create_subscription( + DEFAULT_COMMAND_TOPIC, + rclcpp::SystemDefaultsQoS(), + std::bind(&OmnidirectionalController::velocityCommandStampedCallback, this, _1)); + } else { + vel_cmd_unstamped_subscriber_ = node_->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, + rclcpp::SystemDefaultsQoS(), + std::bind(&OmnidirectionalController::velocityCommandUnstampedCallback, this, _1)); + } + + // initialize odometry publisher and messasge + odometry_publisher_ = node_->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + + // limit the publication on the topics /odom and /tf + publish_rate_ = node_->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + previous_publish_timestamp_ = node_->get_clock()->now(); + + // initialize odom values zeros + odometry_message_.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message_.pose.covariance[diagonal_index] = + odom_params_.pose_covariance_diagonal[index]; + odometry_message_.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + odometry_transform_publisher_ = node_->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + + // keeping track of odom and base_link transforms only + odometry_transform_message_.transforms.resize(1); + odometry_transform_message_.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message_.transforms.front().child_frame_id = odom_params_.base_frame_id; + + return CallbackReturn::SUCCESS; +} + +CallbackReturn OmnidirectionalController::on_activate( + const rclcpp_lifecycle::State & previous_state) { + auto logger = node_->get_logger(); + + RCLCPP_DEBUG(logger, + "Called on_activate. Previous state was %s", + previous_state.label().c_str()); + + if (wheel_names_.empty()) { + RCLCPP_ERROR(logger, "No wheel names specified"); + return CallbackReturn::ERROR; + } + + // register handles + registered_wheel_handles_.reserve(wheel_names_.size()); + for (const auto & wheel_name : wheel_names_) { + std::string interface_name = wheel_name + "/" + HW_IF_VELOCITY; + const auto state_handle = std::find_if(state_interfaces_.cbegin(), state_interfaces_.cend(), + [&interface_name](const auto & interface) { + return interface.get_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return CallbackReturn::ERROR; + } + + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&interface_name](const auto & interface) { + return interface.get_name() == interface_name; + }); + + if (command_handle == command_interfaces_.end()) { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return CallbackReturn::ERROR; + } + + registered_wheel_handles_.emplace_back( + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + + RCLCPP_INFO(logger, "Got command interface: %s", command_handle->get_name().c_str()); + RCLCPP_INFO(logger, "Got state interface: %s", state_handle->get_name().c_str()); + } + + subscriber_is_active_ = true; + + RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); + previous_update_timestamp_ = node_->get_clock()->now(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn OmnidirectionalController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) { + RCLCPP_DEBUG(node_->get_logger(), + "Called on_deactivate. Previous state was %s", + previous_state.label().c_str()); + subscriber_is_active_ = false; + odometry_.reset(); + return CallbackReturn::SUCCESS; +} + +CallbackReturn OmnidirectionalController::on_cleanup( + const rclcpp_lifecycle::State & previous_state) { + RCLCPP_DEBUG(node_->get_logger(), + "Called on_cleanup. Previous state was %s", + previous_state.label().c_str()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn OmnidirectionalController::on_error( + const rclcpp_lifecycle::State & previous_state) { + RCLCPP_DEBUG(node_->get_logger(), + "Called on_error. Previous state was %s", + previous_state.label().c_str()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn OmnidirectionalController::on_shutdown( + const rclcpp_lifecycle::State & previous_state) { + RCLCPP_DEBUG(node_->get_logger(), + "Called on_error. Previous state was %s", + previous_state.label().c_str()); + return CallbackReturn::SUCCESS; +} + +void OmnidirectionalController::velocityCommandStampedCallback( + const geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel) { + if (!subscriber_is_active_) { + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((cmd_vel->header.stamp.sec == 0) && (cmd_vel->header.stamp.nanosec == 0)) { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + cmd_vel->header.stamp = node_->get_clock()->now(); + } + + this->cmd_vel_ = std::move(cmd_vel); +} + +void OmnidirectionalController::velocityCommandUnstampedCallback( + const geometry_msgs::msg::Twist::SharedPtr cmd_vel) { + if (!subscriber_is_active_) { + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + this->cmd_vel_->twist = *cmd_vel; + this->cmd_vel_->header.stamp = node_->get_clock()->now(); +} + +controller_interface::return_type OmnidirectionalController::update( + const rclcpp::Time & time, + const rclcpp::Duration & period) { + auto logger = node_->get_logger(); + const auto current_time = time; + + const auto dt = current_time - cmd_vel_->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (dt > cmd_vel_timeout_) { + cmd_vel_->twist.linear.x = 0.0; + cmd_vel_->twist.linear.y = 0.0; + cmd_vel_->twist.angular.z = 0.0; + } + + if (odom_params_.open_loop) { + odometry_.updateOpenLoop( + {cmd_vel_->twist.linear.x, cmd_vel_->twist.linear.y, cmd_vel_->twist.angular.z}, + period.seconds()); + } else { + std::vector wheels_angular_velocity({0, 0, 0, 0}); + wheels_angular_velocity[0] = registered_wheel_handles_[0].velocity_state.get().get_value(); + wheels_angular_velocity[1] = registered_wheel_handles_[1].velocity_state.get().get_value(); + wheels_angular_velocity[2] = registered_wheel_handles_[2].velocity_state.get().get_value(); + wheels_angular_velocity[3] = registered_wheel_handles_[3].velocity_state.get().get_value(); + try { + odometry_.update(wheels_angular_velocity, period.seconds()); + } catch(const std::runtime_error& e) { + RCLCPP_ERROR(logger, e.what()); + rclcpp::shutdown(); + } + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getPose().theta); + + if (previous_publish_timestamp_ + publish_period_ < current_time) { + previous_publish_timestamp_ += publish_period_; + odometry_message_.header.stamp = current_time; + odometry_message_.pose.pose.position.x = odometry_.getPose().x; + odometry_message_.pose.pose.position.y = odometry_.getPose().y; + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear = cmd_vel_->twist.linear; + odometry_message_.twist.twist.angular = cmd_vel_->twist.angular; + odometry_publisher_->publish(odometry_message_); + } + + // if (odom_params_.enable_odom_tf) { + // odometry_transform_message_.header.stamp = current_time; + // odometry_transform_message_.transform.translation.x = odometry_.getPose().x; + // odometry_transform_message_.transform.translation.y = odometry_.getPose().y; + // odometry_transform_message_.transform.rotation.x = orientation.x(); + // odometry_transform_message_.transform.rotation.y = orientation.y(); + // odometry_transform_message_.transform.rotation.z = orientation.z(); + // odometry_transform_message_.transform.rotation.w = orientation.w(); + // } + + // Compute wheels velocities: + RobotVelocity body_vel_setpoint; + body_vel_setpoint.vx = cmd_vel_->twist.linear.x; + body_vel_setpoint.vy = cmd_vel_->twist.linear.y; + body_vel_setpoint.omega = cmd_vel_->twist.angular.z; + + std::vector wheels_angular_velocity; + wheels_angular_velocity = omni_robot_kinematics_.getWheelsAngularVelocities(body_vel_setpoint); + + // Set wheels velocities: + registered_wheel_handles_[0].velocity_command.get().set_value(wheels_angular_velocity.at(0)); + registered_wheel_handles_[1].velocity_command.get().set_value(wheels_angular_velocity.at(1)); + registered_wheel_handles_[2].velocity_command.get().set_value(wheels_angular_velocity.at(2)); + registered_wheel_handles_[3].velocity_command.get().set_value(wheels_angular_velocity.at(3)); + + return controller_interface::return_type::OK; +} + +OmnidirectionalController::~OmnidirectionalController() {} + +} // namespace omnidirectional_controllers + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + omnidirectional_controllers::OmnidirectionalController, controller_interface::ControllerInterface) diff --git a/src/omnidirectional_controllers/test/test_kinematics.cpp b/src/omnidirectional_controllers/test/test_kinematics.cpp new file mode 100644 index 0000000..5bdafbf --- /dev/null +++ b/src/omnidirectional_controllers/test/test_kinematics.cpp @@ -0,0 +1,109 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include + +#include + +#include "omnidirectional_controllers/kinematics.hpp" +#include "omnidirectional_controllers/types.hpp" + +constexpr double TOLERANCE = 1e-3; + +class TestOmnidirectionalControllersKinematics : public ::testing::Test { + protected: + omnidirectional_controllers::Kinematics kinematics_; +}; + +TEST_F(TestOmnidirectionalControllersKinematics, TestForwardKinematics) { + std::vector angular_velocity; + angular_velocity.reserve(3); + kinematics_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); + + omnidirectional_controllers::RobotVelocity vel; + + angular_velocity = {0.0, 1.63056, -1.63056}; + vel = kinematics_.getBodyVelocity(angular_velocity); + + EXPECT_NEAR(vel.vx, 0.1, TOLERANCE); + EXPECT_NEAR(vel.vy, 0, TOLERANCE); + EXPECT_NEAR(vel.omega, 0, TOLERANCE); + + angular_velocity = {-1.88281, 0.94140, 0.94140}; + vel = kinematics_.getBodyVelocity(angular_velocity); + + EXPECT_NEAR(vel.vx, 0, TOLERANCE); + EXPECT_NEAR(vel.vy, 0.1, TOLERANCE); + EXPECT_NEAR(vel.omega, 0, TOLERANCE); + + angular_velocity = {0.18828, 0.18828, 0.18828}; + vel = kinematics_.getBodyVelocity(angular_velocity); + + EXPECT_NEAR(vel.vx, 0, TOLERANCE); + EXPECT_NEAR(vel.vy, 0, TOLERANCE); + EXPECT_NEAR(vel.omega, 0.1, TOLERANCE); + + angular_velocity = {-1.88281, 2.57196, -0.68915}; + vel = kinematics_.getBodyVelocity(angular_velocity); + + EXPECT_NEAR(vel.vx, 0.1, TOLERANCE); + EXPECT_NEAR(vel.vy, 0.1, TOLERANCE); + EXPECT_NEAR(vel.omega, 0, TOLERANCE); +} + +TEST_F(TestOmnidirectionalControllersKinematics, TestInverseKinematics) { + std::vector angular_velocities; + omnidirectional_controllers::RobotVelocity vel; + kinematics_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); + + vel = {0.1, 0, 0}; + angular_velocities = kinematics_.getWheelsAngularVelocities(vel); + + ASSERT_GT(angular_velocities.size(), 0.0); + EXPECT_NEAR(angular_velocities[0], 0.0, TOLERANCE); + EXPECT_NEAR(angular_velocities[1], 1.63056, TOLERANCE); + EXPECT_NEAR(angular_velocities[2], -1.63056, TOLERANCE); + + vel = {0, 0.1, 0}; + angular_velocities = kinematics_.getWheelsAngularVelocities(vel); + + ASSERT_GT(angular_velocities.size(), 0.0); + EXPECT_NEAR(angular_velocities[0], -1.88281, TOLERANCE); + EXPECT_NEAR(angular_velocities[1], 0.94140, TOLERANCE); + EXPECT_NEAR(angular_velocities[2], 0.94140, TOLERANCE); + + vel = {0, 0, 0.1}; + angular_velocities = kinematics_.getWheelsAngularVelocities(vel); + + ASSERT_GT(angular_velocities.size(), 0.0); + EXPECT_NEAR(angular_velocities[0], 0.18828, TOLERANCE); + EXPECT_NEAR(angular_velocities[1], 0.18828, TOLERANCE); + EXPECT_NEAR(angular_velocities[2], 0.18828, TOLERANCE); + + vel = {0.1, 0.1, 0}; + angular_velocities = kinematics_.getWheelsAngularVelocities(vel); + + ASSERT_GT(angular_velocities.size(), 0.0); + EXPECT_NEAR(angular_velocities[0], -1.88281, TOLERANCE); + EXPECT_NEAR(angular_velocities[1], 2.57196, TOLERANCE); + EXPECT_NEAR(angular_velocities[2], -0.68915, TOLERANCE); +} diff --git a/src/omnidirectional_controllers/test/test_odometry.cpp b/src/omnidirectional_controllers/test/test_odometry.cpp new file mode 100644 index 0000000..dac80d4 --- /dev/null +++ b/src/omnidirectional_controllers/test/test_odometry.cpp @@ -0,0 +1,177 @@ +// MIT License + +// Copyright (c) 2022 Mateus Menezes + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include + +#include +#include +#include + +#include "omnidirectional_controllers/odometry.hpp" +#include "omnidirectional_controllers/types.hpp" + +constexpr double TOLERANCE = 1e-6; + +class TestOmnidirectionalControllersOdometry : public ::testing::Test { + protected: + omnidirectional_controllers::Odometry odom_open_loop_; + omnidirectional_controllers::Odometry odom_dead_reckoning_; + + void testMethodOnlyWithLinearVelocities(std::string method_name) { + ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod(method_name)); + + double time = 0; + constexpr double dt = 0.1; + + omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 0.0}; + + for (size_t i = 0; i < 10; i++) { + time += dt; + odom_open_loop_.updateOpenLoop(vel, dt); + } + + EXPECT_NEAR(time, 1.0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().x, 1.0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 1.0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); + + odom_open_loop_.reset(); + + EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); + } +}; + +TEST_F(TestOmnidirectionalControllersOdometry, TestIfNumericIntegrationMethodIsSetProperly) { + ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("euler_forward")); + ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("runge_kutta2")); + ASSERT_FALSE(odom_open_loop_.setNumericIntegrationMethod("other_method")); +} + +TEST_F(TestOmnidirectionalControllersOdometry, TestIfRobotParamSetThrowsExceptionWithInvalidArgs) { + ASSERT_THROW(odom_open_loop_.setRobotParams({1.0, 0.0, 0.0}), std::runtime_error); + ASSERT_THROW(odom_open_loop_.setRobotParams({0.0, 1.0, 1.0}), std::runtime_error); + ASSERT_NO_THROW(odom_open_loop_.setRobotParams({1.0, 1.0, 0.0})); +} + +TEST_F(TestOmnidirectionalControllersOdometry, + TestEulerMethodOnlyWithLinearVelocitiesInOpenLoop) { + this->testMethodOnlyWithLinearVelocities("euler_forward"); +} + +TEST_F(TestOmnidirectionalControllersOdometry, + TestEulerMethodWithLinearAndAngularVelocitiesInOpenLoop) { + ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("euler_forward")); + + constexpr double dt = 0.1; + omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 1.0}; + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.1, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.1, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.1, TOLERANCE); + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.209484, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.189517, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.2, TOLERANCE); + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.327358, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.267657, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.3, TOLERANCE); + + odom_open_loop_.reset(); + + EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); +} + +TEST_F(TestOmnidirectionalControllersOdometry, + TestRungeKuttaMethodOnlyWithLinearVelocitiesInOpenLoop) { + this->testMethodOnlyWithLinearVelocities("runge_kutta2"); +} + +TEST_F(TestOmnidirectionalControllersOdometry, + TestRungeKuttaMethodWithLinearAndAngularVelocitiesInOpenLoop) { + ASSERT_TRUE(odom_open_loop_.setNumericIntegrationMethod("runge_kutta2")); + + constexpr double dt = 0.1; + omnidirectional_controllers::RobotVelocity vel = {1.0, 1.0, 1.0}; + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.104873, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.094877, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.1, TOLERANCE); + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.218694, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.178810, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.2, TOLERANCE); + + odom_open_loop_.updateOpenLoop(vel, dt); + EXPECT_NEAR(odom_open_loop_.getPose().x, 0.340326, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0.250961, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0.3, TOLERANCE); + + odom_open_loop_.reset(); + + EXPECT_NEAR(odom_open_loop_.getPose().x, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().y, 0, TOLERANCE); + EXPECT_NEAR(odom_open_loop_.getPose().theta, 0, TOLERANCE); +} + +TEST_F(TestOmnidirectionalControllersOdometry, + TestRungeKuttaMethodWithLinearAndAngularVelocitiesInDeadReckoning) { + ASSERT_TRUE(odom_dead_reckoning_.setNumericIntegrationMethod("runge_kutta2")); + + constexpr double dt = 0.1; + std::vector wheels_vel{-16.94525768606293, 27.60242026826863, -5.008743353518062}; + odom_dead_reckoning_.setRobotParams({0.053112205, 0.1, DEG2RAD(30)}); + + ASSERT_NO_THROW(odom_dead_reckoning_.update(wheels_vel, dt)); + ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().vx, 1.0, TOLERANCE); + ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().vy, 1.0, TOLERANCE); + ASSERT_NEAR(odom_dead_reckoning_.getBodyVelocity().omega, 1.0, TOLERANCE); + + EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.104873, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.094877, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.1, TOLERANCE); + + odom_dead_reckoning_.update(wheels_vel, dt); + EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.218694, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.178810, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.2, TOLERANCE); + + odom_dead_reckoning_.update(wheels_vel, dt); + EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0.340326, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0.250961, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0.3, TOLERANCE); + + odom_dead_reckoning_.reset(); + + EXPECT_NEAR(odom_dead_reckoning_.getPose().x, 0, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().y, 0, TOLERANCE); + EXPECT_NEAR(odom_dead_reckoning_.getPose().theta, 0, TOLERANCE); +}