Clase 11 - Taller de resolución

Odometría

Cálculo de odometría para un robot diferencial

  • Distancia recorrida por la rueda derecha: \[ d_{R_k} = (\textcolor{Plum}{\phi_{R_k}} - \textcolor{Plum}{\phi_{R_{k-1}}}) r \]

  • Distancia recorrida por la rueda izquierda: \[ d_{L_k} = (\textcolor{Plum}{\phi_{L_k}} - \textcolor{Plum}{\phi_{L_{k-1}}}) r \]

  • Distancia recorrida por el robot: \[ \textcolor{Blue}{\Delta d_{k}} = \frac{d_{R_k} + d_{L_k}}{2} \]

  • Desplazamiento angular del robot: \[ \textcolor{Blue}{\Delta \theta_k} = \frac{d_{R_k} - d_{L_k}}{b} \]

  • Cálculo de odometría: \[ \boldsymbol{\xi}(t_{k+1}) \approx \boldsymbol{\xi}(t_k) + \dot{\boldsymbol{\xi}}(t_k)(t_{k+1}-t_{k}) \]

\[ {}^{O} \boldsymbol{\xi}_{R} (t_{k+1}) = \begin{bmatrix} \textcolor{ForestGreen}{{x}_{k+1}}\\ \textcolor{ForestGreen}{{y}_{k+1}}\\ \textcolor{Orange}{\theta_{k+1}} \end{bmatrix} \approx \begin{bmatrix} \textcolor{ForestGreen}{{x}_{k}}\\ \textcolor{ForestGreen}{{y}_{k}}\\ \textcolor{Orange}{\theta_{k}} \end{bmatrix} + \begin{bmatrix} \cos{\textcolor{Orange}{\theta_k}} & 0\\ \sin{\textcolor{Orange}{\theta_k}} & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix} \textcolor{Blue}{\Delta d_{k}}\\ \textcolor{Blue}{\Delta \theta_k} \end{bmatrix} \]

\[ {}^{O} \boldsymbol{\xi}_{R} (t_{k+1}) \begin{cases} \textcolor{ForestGreen}{{x}_{k+1}} \approx \textcolor{ForestGreen}{{x}_{k}} + \textcolor{Blue}{\Delta d_{k}} \cos{\textcolor{Orange}{\theta_k}} \\[.5em] \textcolor{ForestGreen}{{y}_{k+1}} \approx \textcolor{ForestGreen}{{y}_{k}} + \textcolor{Blue}{\Delta d_{k}} \sin{\textcolor{Orange}{\theta_k}} \\[.5em] \textcolor{Orange}{\theta_{k+1}} \approx \textcolor{Orange}{\theta_{k}} + \textcolor{Blue}{\Delta \theta_k} \end{cases} \]

Resolución ejercicios 9 al 12

Crear un nodo (en el mismo paquete que el ejercicio anterior o por separado) que lea el o los topics de posición de las juntas de las ruedas (publicados por el JointStateBroadcaster), calcule la odometría basado en el modelo cinemático directo del robot y publique la misma en el topic \odom con mensajes de tipo nav_msgs/Odometry. El nodo debe recibir como parámetro las dimensiones geométricas del modelo cinemático y los nombres correspondientes a las juntas de las ruedas de tracción.

Note

El campo twist y covariance en pose no se deben completar para el mensaje nav_msgs/Odometry.

Cree un nuevo paquete (*)_bringup con un archivo launch en el cual se cargue todo el sistema desarrollado (URDF, Gazebo, los controladores de ROS2 control) y los nodos de los ejercicios 8 y 9. Recuerde configurar correctamente los parámetros y topics de cada nodo. Puede probar que el robot se encuentra funcionando utilizando el teleop_twist_keyboard o publicando mensajes de Twist como los del ejercicio 5 o 6.

  • (Opción A) Crear un nodo que publique la transformación entre los frames odom y base_link a partir de los mensajes del topic /odom. Agregar la ejecución del mismo al launch del ejercicio anterior.

  • (Opción B) Modificar el nodo del ejercicio 9 para que que publique la transformación entre los frames odom y base_link. Agregar un parámetro para que cuando no se requiera esta transformación, no se publique.

Cree un archivo launch en el paquete (*)_bringup que extienda (inclusión o copia) al del ejercicio 10 y añada la ejecución de RViz junto con su respectivo archivo de configuración, el cual muestre el robot en el frame odom y las transformaciones de todos los links definidos en el URDF.