Fix state and command interfaces register bug

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<br />
 **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

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}

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()) {