Navegación Basada en Odometría (Move Square)

Un node combinado de publisher y subscriber para lograr control basado en odometría...

A continuación encontrarás una plantilla de script de Python para mostrarte cómo puedes publicar en /cmd_vel y suscribirte a /odom en el mismo node. Esto te ayudará a construir un controlador de lazo cerrado para hacer que tu robot siga una trayectoria de movimiento cuadrada de tamaño: 1m x 1m.

Puedes publicar comandos de velocidad en /cmd_vel para hacer que el robot se mueva, monitorear la posición y orientación del robot en tiempo real, determinar cuándo se ha completado el movimiento deseado, y luego actualizar los comandos de velocidad en consecuencia.

Enfoque Sugerido¶

Moverse en un cuadrado puede lograrse alternando entre dos estados de movimiento diferentes de forma secuencial: Avanzar y girar en el lugar. Al inicio de cada paso de movimiento podemos leer la odometría actual del robot, y luego usarla como referencia para comparar y saber cuándo la posición/orientación del robot ha cambiado en la cantidad requerida, por ejemplo:

Con el robot estacionario, lee la odometría para determinar su posición X e Y actual en el entorno.
Avanza hasta que la posición X e Y del robot indiquen que se ha movido 1m hacia adelante.
Detén el movimiento hacia adelante.
Lee la odometría del robot para determinar su orientación actual ("yaw"/θ_z).
Gira en el lugar hasta que la orientación del robot cambie 90° (o el equivalente en radianes).
Detén el giro.
Repite.

El Código¶

move_square.py

import rclpy
from rclpy.node import Node
from rclpy.signals import SignalHandlerOptions

from geometry_msgs.msg import TwistStamped # (1)!
from nav_msgs.msg import Odometry # (2)!

from part2_navigation_modules.tb3_tools import quaternion_to_euler # (3)!
from math import sqrt, pow, pi # (4)!

class Square(Node):

    def __init__(self):
        super().__init__("move_square")

        self.first_message = False
        self.turn = False 

        self.vel_msg = TwistStamped() # (5)! 
        # (6)!
        self.x = 0.0; self.y = 0.0; self.theta_z = 0.0
        self.xref = 0.0; self.yref = 0.0; self.theta_zref = 0.0
        # (7)!
        self.yaw = 0.0 
        self.displacement = 0.0 

        self.vel_pub = self.create_publisher(
            msg_type=TwistStamped,
            topic="cmd_vel",
            qos_profile=10,
        )

        self.odom_sub = self.create_subscription(
            msg_type=Odometry,
            topic="odom",
            callback=self.odom_callback,
            qos_profile=10,
        )

        ctrl_rate = 10 # hz
        self.timer = self.create_timer(
            timer_period_sec=1/ctrl_rate,
            callback=self.timer_callback,
        )

        self.shutdown = False

        self.get_logger().info(
            f"The '{self.get_name()}' node is initialised."
        )

    def on_shutdown(self):
        print("Stopping the robot...")
        self.vel_pub.publish(TwistStamped())
        self.shutdown = True

    def odom_callback(self, msg_data: Odometry):
        pose = msg_data.pose.pose 

        (roll, pitch, yaw) = quaternion_to_euler(pose.orientation) # (8)!

        self.x = pose.position.x # (9)!
        self.y = pose.position.y
        self.theta_z = abs(yaw) # abs(yaw) makes life much easier!!

        if not self.first_message: # (10)!
            self.first_message = True
            self.xref = self.x
            self.yref = self.y
            self.theta_zref = self.theta_z

    def timer_callback(self):
        # here is where the code to control the motion of the robot
        # goes. Add code here to make the robot move in a square of
        # dimensions 1 x 1m...
        if self.turn:
            # turn by 90 degrees...
            pass

        else:
            # move forwards by 1m...
            pass

        # publish whatever velocity command has been set above:
        self.vel_pub.publish(self.vel_msg)

def main(args=None):
    rclpy.init(
        args=args,
        signal_handler_options=SignalHandlerOptions.NO,
    )
    move_square = Square()
    try:
        rclpy.spin(move_square)
    except KeyboardInterrupt:
        print(
            f"{move_square.get_name()} received a shutdown request (Ctrl+C)."
        )
    finally:
        move_square.on_shutdown()
        while not move_square.shutdown:
            continue
        move_square.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

Importa el mensaje TwistStamped para publicar comandos de velocidad en /cmd_vel.
Importa el mensaje Odometry, para usar al suscribirse al topic /odom.
Importa el método quaternion_to_euler de tb3_tools.py...

Este es un método útil que está incluido en la Plantilla de Package ROS 2 para convertir la orientación de cuaterniones a ángulos de Euler (sobre los ejes principales).

¡Cualquier package de ROS 2 que crees con nuestra plantilla de package ROS 2 contendrá este método para que lo uses!
Finalmente, importa algunas operaciones matemáticas útiles (y pi), que pueden resultar útiles para esta tarea:

Operación Matemática Implementación en Python

\(\sqrt{a+b}\) sqrt(a+b)

\(a^{2}+(bc)^{3}\) pow(a, 2) + pow(b*c, 3)

\(\pi r^2\) pi * pow(r, 2)
Aquí establecemos un mensaje TwistStamped, que podemos poblar con velocidades y luego publicar en /cmd_vel dentro del método timer_callback() (para hacer que el robot se mueva).
Aquí definimos algunas variables que podemos usar para almacenar bits relevantes de datos de odometría mientras nuestro node está corriendo (y leerlos de vuelta para implementar control con retroalimentación):
- self.x, self.y y self.theta_z serán usados por el odom_callback() para almacenar la pose actual del robot
- self.xref, self.yref y self.theta_zref pueden usarse en el método timer_callback() para llevar un registro de dónde estaba el robot en un momento dado (y determinar cuánto se ha movido desde ese punto)
También necesitaremos hacer un seguimiento de cuánto ha viajado el robot (o girado) para determinar cuándo ha ocurrido suficiente movimiento para desencadenar un cambio al estado alternativo, es decir:
- if viajó 1 metro, then: girar
- if giró 90°, then: avanzar
Aquí obtenemos la orientación actual del robot (en cuaterniones) y la convertimos a ángulos de Euler (en radianes) sobre los ejes principales, donde:
- "roll" = θ_x
- "pitch" = θ_y
- "yaw" = θ_z
Solo nos interesan x, y y θ_z, así que los asignamos a variables de clase self.x, self.y y self.theta_z, para que podamos acceder a ellos en cualquier parte dentro de nuestra clase Square().
A veces, puede tomar unos momentos para que llegue el primer mensaje del topic, y es útil saber cuándo ha ocurrido eso para saber que estás tratando con datos reales del topic. Aquí, simplemente estamos estableciendo una bandera en True una vez que la función callback ha ejecutado por primera vez (es decir, el primer mensaje del topic ha sido recibido).

Enfoque Alternativo: Seguimiento de Puntos de Referencia¶

Una trayectoria de movimiento cuadrada puede definirse completamente por las coordenadas de sus cuatro esquinas, y podemos hacer que el robot se mueva hacia cada una de estas esquinas una por una, usando su sistema de odometría para monitorear su posición en tiempo real, y adaptando las velocidades lineales y angulares en consecuencia.

Esto es un poco más complicado, y es posible que quieras esperar hasta que tengas un poco más de experiencia con ROS antes de abordarlo de esta manera.

Operación Matemática	Implementación en Python
\(\sqrt{a+b}\)	`sqrt(a+b)`
\(a^{2}+(bc)^{3}\)	`pow(a, 2) + pow(b*c, 3)`
\(\pi r^2\)	`pi * pow(r, 2)`