From dd54b9c8a6b0a48ca9ff360b27404e4e1385b6ec Mon Sep 17 00:00:00 2001 From: Mateus Menezes Date: Tue, 16 Jan 2024 08:30:04 -0300 Subject: [PATCH] Fix state and command interfaces register bug (#5) * Fix bug in handling state interfaces names * Update documentation --- README.md | 13 ++++++++----- doc/kinematics_and_odometry.md | 8 ++++---- src/omnidirectional_controller.cpp | 14 +++++++------- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 49d2486..f7df6b0 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ 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). +See the documentation [Omnidirectional Robot Kinematics and Odometry](doc/kinematics_and_odometry.md) for more details. + **Author:** Mateus Menezes
**Maintainer:** Mateus Menezes, mateusmenezes95@gmail.com @@ -13,9 +15,9 @@ ROS2 Distro | Branch | Build status | ## Installation Premises -1. This repository has been tested on [ROS2 Foxy] and with [Classic Gazebo 11]; +1. This repository has been tested on [ROS2 Humble] 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]; +2. These instructions assume that you have already installed ROS2 Humble Hawskbill 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. @@ -37,7 +39,7 @@ cd ~/ros_ws rosdep init rosdep update sudo apt update -rosdep install --from-paths src/omnidirectional_controllers --ignore-src -r -y --rosdistro foxy +rosdep install --from-paths src/omnidirectional_controllers --ignore-src -r -y --rosdistro humble ``` If all dependencies are already installed, you should see the message "All required rosdeps installed successfully." @@ -52,6 +54,7 @@ colcon build --symlink-install --event-handlers console_direct+ ``` > Run `colcon build --help` to understand the arguments passed! +> If you want to generate the compile_commands.json file, add the argument `--cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1` to the command above. After building the package, open a new terminal and navigate to your workspace. Then, source the overlay by running the following command: @@ -96,8 +99,8 @@ Please report bugs and request features using the [Issue Tracker] [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 +[Creating a workspace tutorial]: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html +[recommended ubuntu installation tutorial]: https://docs.ros.org/en/humble/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 diff --git a/doc/kinematics_and_odometry.md b/doc/kinematics_and_odometry.md index b0672fc..3ba0984 100644 --- a/doc/kinematics_and_odometry.md +++ b/doc/kinematics_and_odometry.md @@ -170,11 +170,11 @@ The robot’s velocity with regard to the world (w.r.t) is given by v\\ v_{n}\\ \omega - \end{bmatrix}1 + \end{bmatrix} \end{equation} ``` -where $R(\theta)$ is the rotation matrix around the $z$ axis perpendicular de horizontal plane $xy$, given by +where $R(\theta)$ is the rotation matrix around the $z$ axis perpendicular to the horizontal plane $xy$, given by ```math \begin{equation} @@ -188,9 +188,9 @@ where $R(\theta)$ is the rotation matrix around the $z$ axis perpendicular de ho \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]. +Assume that the robot configuration $q(t_k) = 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 $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 +Considering that the sampling period $T_s = t_{k+1} - t_k$, the Euler method for the axebot configuration $q_{k+1}$ is given by ```math \begin{align} diff --git a/src/omnidirectional_controller.cpp b/src/omnidirectional_controller.cpp index 3f2fda3..3059c88 100644 --- a/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controller.cpp @@ -229,10 +229,11 @@ CallbackReturn OmnidirectionalController::on_activate( // register handles registered_wheel_handles_.reserve(wheel_names_.size()); for (const auto & wheel_name : wheel_names_) { - const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name](const auto & interface) { - return interface.get_name() == wheel_name && - interface.get_interface_name() == HW_IF_VELOCITY; + 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()) { @@ -242,9 +243,8 @@ CallbackReturn OmnidirectionalController::on_activate( const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&wheel_name](const auto & interface) { - return interface.get_name() == wheel_name && - interface.get_interface_name() == HW_IF_VELOCITY; + [&interface_name](const auto & interface) { + return interface.get_name() == interface_name; }); if (command_handle == command_interfaces_.end()) {